Skip to content

Commit

Permalink
added imu extrinsics reading
Browse files Browse the repository at this point in the history
  • Loading branch information
@Serafadam
Serafadam committed Aug 2, 2023
1 parent c356582 commit 5cc8a72
Show file tree
Hide file tree
Showing 5 changed files with 74 additions and 53 deletions.
6 changes: 4 additions & 2 deletions depthai_bridge/include/depthai_bridge/TFPublisher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,8 +9,10 @@ namespace dai {
namespace ros {
class TFPublisher{
public:
explicit TFPublisher(rclcpp::Node *node, dai::CalibrationHandler handler);
std::string getXacro();
explicit TFPublisher(rclcpp::Node *node, dai::CalibrationHandler handler, const std::string& xacroArgs);
std::string getXacro(const std::string& xacroArgs);
geometry_msgs::msg::Quaternion quatFromRotM(nlohmann::json rotMatrix);
geometry_msgs::msg::Vector3 transFromExtr(nlohmann::json translation);
private:
std::unique_ptr<rclcpp::AsyncParametersClient> paramClient;
std::shared_ptr<tf2_ros::StaticTransformBroadcaster> tfPub;
Expand Down
101 changes: 63 additions & 38 deletions depthai_bridge/src/TFPublisher.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,23 +1,22 @@
#include "depthai_bridge/TFPublisher.hpp"

#include "ament_index_cpp/get_package_share_directory.hpp"
#include "geometry_msgs/msg/quaternion.hpp"
#include "geometry_msgs/msg/transform_stamped.hpp"
#include "nlohmann/json.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2/LinearMath/Matrix3x3.h"
#include "tf2/LinearMath/Quaternion.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs/tf2_geometry_msgs.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
namespace dai {
namespace ros {
TFPublisher::TFPublisher(rclcpp::Node* node, dai::CalibrationHandler handler) {
TFPublisher::TFPublisher(rclcpp::Node* node, dai::CalibrationHandler handler, const std::string& xacroArgs) {
tfPub = std::make_shared<tf2_ros::StaticTransformBroadcaster>(node);
auto json = handler.eepromToJson();
auto camData = json["cameraData"];
std::unordered_map<int, std::string> socketNameMap{{0, "rgb"}, {1, "left"}, {2, "right"}};
paramClient = std::make_unique<rclcpp::AsyncParametersClient>(node, node->get_name() + std::string("_state_publisher"));
auto urdf = getXacro();
// RCLCPP_INFO(node->get_logger(), "%s", urdf.c_str());
auto urdf = getXacro(xacroArgs);
auto urdf_param = rclcpp::Parameter("robot_description", urdf);

paramClient->set_parameters({urdf_param});
Expand All @@ -26,37 +25,17 @@ TFPublisher::TFPublisher(rclcpp::Node* node, dai::CalibrationHandler handler) {
geometry_msgs::msg::TransformStamped optical_ts;
ts.header.stamp = node->get_clock()->now();
optical_ts.header.stamp = ts.header.stamp;
tf2::Matrix3x3 m(cam[1]["extrinsics"]["rotationMatrix"][0][0],
cam[1]["extrinsics"]["rotationMatrix"][0][1],
cam[1]["extrinsics"]["rotationMatrix"][0][2],

cam[1]["extrinsics"]["rotationMatrix"][1][0],
cam[1]["extrinsics"]["rotationMatrix"][1][1],
cam[1]["extrinsics"]["rotationMatrix"][1][2],
auto extrinsics = cam[1]["extrinsics"];

cam[1]["extrinsics"]["rotationMatrix"][2][0],
cam[1]["extrinsics"]["rotationMatrix"][2][1],
cam[1]["extrinsics"]["rotationMatrix"][2][2]);
ts.transform.rotation = quatFromRotM(extrinsics["rotationMatrix"]);
ts.transform.translation = transFromExtr(extrinsics["translation"])

tf2::Quaternion q;
m.getRotation(q);
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(q);
ts.transform.rotation = msg_quat;
// optical coordinates to ROS
ts.transform.translation.x = cam[1]["extrinsics"]["translation"]["y"];
ts.transform.translation.x /= -100.0;
ts.transform.translation.y = cam[1]["extrinsics"]["translation"]["x"];
ts.transform.translation.y /= -100.0;
ts.transform.translation.z = cam[1]["extrinsics"]["translation"]["z"];
ts.transform.translation.z /= 100.0;

std::string name = socketNameMap.at(cam[0]);
std::string name = socketNameMap.at(cam[0]);
ts.child_frame_id = node->get_name() + std::string("_") + name + std::string("_camera_frame");
optical_ts.child_frame_id = node->get_name() + std::string("_") + name + std::string("_camera_optical_frame");
optical_ts.header.frame_id = ts.child_frame_id;

// check if the camera is at the end of the chain
if(cam[1]["extrinsics"]["toCameraSocket"] != -1) {
ts.header.frame_id = node->get_name() + std::string("_") + socketNameMap.at(cam[1]["extrinsics"]["toCameraSocket"]) + std::string("_camera_frame");
if(extrinsics["toCameraSocket"] != -1) {
ts.header.frame_id = node->get_name() + std::string("_") + socketNameMap.at(extrinsics["toCameraSocket"]) + std::string("_camera_frame");
} else {
ts.header.frame_id = node->get_name();
ts.transform.rotation.w = 1.0;
Expand All @@ -65,6 +44,8 @@ TFPublisher::TFPublisher(rclcpp::Node* node, dai::CalibrationHandler handler) {
ts.transform.rotation.z = 0.0;
}
// rotate optical fransform
optical_ts.child_frame_id = node->get_name() + std::string("_") + name + std::string("_camera_optical_frame");
optical_ts.header.frame_id = ts.child_frame_id;
optical_ts.transform.rotation.w = 0.5;
optical_ts.transform.rotation.x = -0.5;
optical_ts.transform.rotation.y = 0.5;
Expand All @@ -73,22 +54,66 @@ TFPublisher::TFPublisher(rclcpp::Node* node, dai::CalibrationHandler handler) {
tfPub->sendTransform(optical_ts);
}

geometry_msgs::msg::TransformStamped ts;
ts.header.stamp = node->get_clock()->now();
auto imuExtr = json["imuExtrinsics"];
if(imuExtr["toCameraSocket"] != -1) {
ts.header.frame_id = node->get_name() + std::string("_") + socketNameMap.at(extrinsics["toCameraSocket"]) + std::string("_camera_frame");
} else {
ts.header.frame_id = node->get_name();
ts.transform.rotation.w = 1.0;
ts.transform.rotation.x = 0.0;
ts.transform.rotation.y = 0.0;
ts.transform.rotation.z = 0.0;
}
ts.child_frame_id = node->get_name() + std::string("_imu_frame");

ts.transform.rotation = quatFromRotM(imuExtr["rotationMatrix"]);
ts.transform.translation = transFromExtr(imuExtr["translation"]);
}

geometry_msgs::msg::Vector3 transFromExtr(nlohmann::json translation) {
geometry_msgs::msg::Vector3 trans;
// optical coordinates to ROS
trans.x = translation["y"];
trans.x /= -100.0;
trans.y = translation["x"];
trans.y /= -100.0;
trans.z = translation["z"];
trans.z /= 100.0;
}
geometry_msgs::msg::Quaternion TFPublisher::quatFromRotM(nlohmann::json rotMatrix) {
tf2::Matrix3x3 m(rotMatrix[0][0],
rotMatrix[0][1],
rotMatrix[0][2],

std::string TFPublisher::getXacro(){
// auto path = ament_index_cpp::get_package_share_directory("depthai_descriptions");
const char* cmd = "xacro /workspaces/ros_2/src/depthai-ros/depthai_descriptions/urdf/depthai_descr.urdf.xacro camera_name:=oak camera_model:=OAK-D-PRO base_frame:=oak-d_frame parent_frame:=map cam_pos_x:=2.0";
rotMatrix[1][0],
rotMatrix[1][1],
rotMatrix[1][2],

rotMatrix[2][0],
rotMatrix[2][1],
rotMatrix[2][2]);

tf2::Quaternion q;
m.getRotation(q);
geometry_msgs::msg::Quaternion msg_quat = tf2::toMsg(q);
return msg_quat;
}
std::string TFPublisher::getXacro(const std::string& xacroArgs) {
auto path = ament_index_cpp::get_package_share_directory("depthai_descriptions");
std::string cmd = "xacro /workspaces/ros_2/src/depthai-ros/depthai_descriptions/urdf/depthai_descr.urdf.xacro ";
cmd += xacroArgs;
std::array<char, 128> buffer;
std::string result;
std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(cmd, "r"), pclose);
if (!pipe) {
std::unique_ptr<FILE, decltype(&pclose)> pipe(popen(cmd.c_str(), "r"), pclose);
if(!pipe) {
throw std::runtime_error("popen() failed!");
}
while (fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) {
while(fgets(buffer.data(), buffer.size(), pipe.get()) != nullptr) {
result += buffer.data();
}
return result;

}
} // namespace ros
} // namespace dai
Empty file added depthai_descriptions/urdf/include/base.urdf.xacro
View file
Empty file.
4 changes: 3 additions & 1 deletion depthai_ros_driver/src/camera.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -26,7 +26,9 @@ void Camera::onConfigure() {
stopSrv = this->create_service<Trigger>("~/stop_camera", std::bind(&Camera::stopCB, this, std::placeholders::_1, std::placeholders::_2));
savePipelineSrv = this->create_service<Trigger>("~/save_pipeline", std::bind(&Camera::savePipelineCB, this, std::placeholders::_1, std::placeholders::_2));
saveCalibSrv = this->create_service<Trigger>("~/save_calibration", std::bind(&Camera::saveCalibCB, this, std::placeholders::_1, std::placeholders::_2));
tfPub = std::make_unique<dai::ros::TFPublisher>(this, device->readCalibration());
if(ph->getParam<bool>("i_publish_tf_from_calibration")){
tfPub = std::make_unique<dai::ros::TFPublisher>(this, device->readCalibration(), ph->getParam<std::string>("i_tf_publisher_xacro_args"));
}
RCLCPP_INFO(this->get_logger(), "Camera ready!");
}

Expand Down
16 changes: 4 additions & 12 deletions depthai_ros_driver/src/param_handlers/camera_param_handler.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,19 +35,11 @@ void CameraParamHandler::declareParams() {
declareAndLogParam<std::string>("i_external_calibration_path", "");
declareAndLogParam<int>("i_laser_dot_brightness", 800, getRangedIntDescriptor(0, 1200));
declareAndLogParam<int>("i_floodlight_brightness", 0, getRangedIntDescriptor(0, 1500));

declareAndLogParam<bool>("i_publish_tf_from_calibration", false);
declareAndLogParam<std::string>("i_camera_name", "oak");
declareAndLogParam<std::string>("i_camera_model", "OAK-D");
declareAndLogParam<std::string>("i_base_frame", "oak-d_frame");
declareAndLogParam<std::string>("i_parent_frame", "oak-d-base-frame");
declareAndLogParam<std::string>("i_cam_pos_x", "0.0");
declareAndLogParam<std::string>("i_cam_pos_y", "0.0");
declareAndLogParam<std::string>("i_cam_pos_z", "0.0");
declareAndLogParam<std::string>("i_cam_roll", "1.5708");
declareAndLogParam<std::string>("i_cam_pitch", "0.0");
declareAndLogParam<std::string>("i_cam_yaw", "1.5708");

declareAndLogParam<bool>("i_publish_tf_from_calibration", true);
declareAndLogParam<std::string>("i_tf_publisher_xacro_args",
"camera_name:=oak camera_model:=OAK-D base_frame:=oak-d_frame parent_frame:=map cam_pos_x:=0.0 cam_pos_y:=0.0 "
"cam_pos_z:=0.0 cam_roll:=1.5708 cam_pitch:=0.0 cam_yaw:=1.5708");
}
dai::CameraControl CameraParamHandler::setRuntimeParams(const std::vector<rclcpp::Parameter>& /*params*/) {
dai::CameraControl ctrl;
Expand Down

0 comments on commit 5cc8a72

Please sign in to comment.