Skip to content

Commit

Permalink
Patch to add namespaces and to correctly handle node name, namespace …
Browse files Browse the repository at this point in the history 
…and camera name
  • Loading branch information
@icosac
icosac committed Dec 6, 2024
1 parent 2a65533 commit d057f2b
Show file tree
Hide file tree
Showing 6 changed files with 42 additions and 17 deletions.
12 changes: 9 additions & 3 deletions realsense2_camera/include/base_realsense_node.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -41,9 +41,13 @@
using realsense2_camera_msgs::msg::Extrinsics;
using realsense2_camera_msgs::msg::IMUInfo;

#define FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_frame")).str()
#define OPTICAL_FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_optical_frame")).str()
#define ALIGNED_DEPTH_TO_FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << "aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str()
// #define FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_frame")).str()
// #define OPTICAL_FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_optical_frame")).str()
// #define ALIGNED_DEPTH_TO_FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << "aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str()

#define FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _tf_ns << "/" << _camera_name << (_camera_name != "" ? "/" : "") << STREAM_NAME(sip) << "_frame")).str()
#define OPTICAL_FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _tf_ns << "/" << _camera_name << (_camera_name != "" ? "/" : "") << STREAM_NAME(sip) << "_optical_frame")).str()
#define ALIGNED_DEPTH_TO_FRAME_ID(sip) (static_cast<std::ostringstream&&>(std::ostringstream() << _tf_ns << "/" << _camera_name << (_camera_name != "" ? "/" : "") << "aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str()

namespace realsense2_camera
{
Expand Down Expand Up @@ -127,6 +131,8 @@ namespace realsense2_camera
}
};

std::string _tf_ns;
std::string _odom_tf;
std::string _base_frame_id;
bool _is_running;
rclcpp::Node& _node;
Expand Down
2 changes: 1 addition & 1 deletion realsense2_camera/include/constants.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -98,7 +98,7 @@ namespace realsense2_camera


const std::string DEFAULT_BASE_FRAME_ID = "link";
const std::string DEFAULT_ODOM_FRAME_ID = "odom_frame";
const std::string DEFAULT_ODOM_FRAME_ID = "odom_frame_default";
const std::string DEFAULT_IMU_OPTICAL_FRAME_ID = "camera_imu_optical_frame";

const std::string DEFAULT_UNITE_IMU_METHOD = "";
Expand Down
22 changes: 13 additions & 9 deletions realsense2_camera/launch/rs_launch.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,9 @@
from launch.conditions import IfCondition


configurable_parameters = [{'name': 'camera_name', 'default': 'camera', 'description': 'camera unique name'},
configurable_parameters = [{'name': 'camera_name', 'default': '', 'description': 'camera unique name'},
{'name': 'node_name', 'default': "t265_node", 'description': 'The name of the ROS2 node'},
{'name': 'node_namespace', 'default': "", 'description': 'The namespace of the ROS2 node'},
{'name': 'serial_no', 'default': "''", 'description': 'choose device by serial number'},
{'name': 'usb_port_id', 'default': "''", 'description': 'choose device by usb port id'},
{'name': 'device_type', 'default': "''", 'description': 'choose device by type'},
Expand Down Expand Up @@ -60,6 +62,8 @@
{'name': 'depth_module.gain.2', 'default': '16', 'description': 'Initial value for hdr_merge filter'},
{'name': 'wait_for_device_timeout', 'default': '-1.', 'description': 'Timeout for waiting for device to connect (Seconds)'},
{'name': 'reconnect_timeout', 'default': '6.', 'description': 'Timeout(seconds) between consequtive reconnection attempts'},
{'name': 'tf_ns', 'default': 'tf_launch_ns', 'description': 'The tf namespace'},
{'name': 'odom_tf', 'default': 'odom_tf', 'description': 'The tf for odom_frame (WITH namespace)'},
]

def declare_configurable_parameters(parameters):
Expand All @@ -76,8 +80,8 @@ def generate_launch_description():
launch_ros.actions.Node(
condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " == ''"])),
package='realsense2_camera',
node_namespace=LaunchConfiguration("camera_name"),
node_name=LaunchConfiguration("camera_name"),
node_namespace=LaunchConfiguration("node_namespace"),
node_name=LaunchConfiguration("node_name"),
node_executable='realsense2_camera_node',
prefix=['stdbuf -o L'],
parameters=[set_configurable_parameters(configurable_parameters)
Expand All @@ -88,8 +92,8 @@ def generate_launch_description():
launch_ros.actions.Node(
condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " != ''"])),
package='realsense2_camera',
node_namespace=LaunchConfiguration("camera_name"),
node_name=LaunchConfiguration("camera_name"),
node_namespace=LaunchConfiguration("node_namespace"),
node_name=LaunchConfiguration("node_name"),
node_executable='realsense2_camera_node',
prefix=['stdbuf -o L'],
parameters=[set_configurable_parameters(configurable_parameters)
Expand All @@ -105,8 +109,8 @@ def generate_launch_description():
launch_ros.actions.Node(
condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " == ''"])),
package='realsense2_camera',
namespace=LaunchConfiguration("camera_name"),
name=LaunchConfiguration("camera_name"),
namespace=LaunchConfiguration("node_namespace"),
name=LaunchConfiguration("node_name"),
executable='realsense2_camera_node',
parameters=[set_configurable_parameters(configurable_parameters)
],
Expand All @@ -117,8 +121,8 @@ def generate_launch_description():
launch_ros.actions.Node(
condition=IfCondition(PythonExpression([LaunchConfiguration('config_file'), " != ''"])),
package='realsense2_camera',
namespace=LaunchConfiguration("camera_name"),
name=LaunchConfiguration("camera_name"),
namespace=LaunchConfiguration("node_namespace"),
name=LaunchConfiguration("node_name"),
executable='realsense2_camera_node',
parameters=[set_configurable_parameters(configurable_parameters)
, PythonExpression([LaunchConfiguration("config_file")])
Expand Down
2 changes: 1 addition & 1 deletion realsense2_camera/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -17,7 +17,7 @@
<depend>builtin_interfaces</depend>
<depend>cv_bridge</depend>
<depend>image_transport</depend>
<depend>librealsense2</depend>
<!-- <depend>librealsense2</depend> -->
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>realsense2_camera_msgs</depend>
Expand Down
11 changes: 9 additions & 2 deletions realsense2_camera/src/base_realsense_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -465,7 +465,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame)
static tf2_ros::TransformBroadcaster br(_node);
geometry_msgs::msg::TransformStamped msg;
msg.header.stamp = t;
msg.header.frame_id = DEFAULT_ODOM_FRAME_ID;
msg.header.frame_id = this->_odom_tf;
msg.child_frame_id = FRAME_ID(POSE);
msg.transform.translation.x = pose_msg.pose.position.x;
msg.transform.translation.y = pose_msg.pose.position.y;
Expand All @@ -475,6 +475,9 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame)
msg.transform.rotation.z = pose_msg.pose.orientation.z;
msg.transform.rotation.w = pose_msg.pose.orientation.w;

// std::cout << "Sending tf broadcast with frame_id: " << msg.header.frame_id
// << " and child_frame_id: " << msg.child_frame_id << std::endl;

if (_publish_odom_tf) br.sendTransform(msg);

if (0 != _odom_publisher->get_subscription_count())
Expand All @@ -499,7 +502,7 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame)

nav_msgs::msg::Odometry odom_msg;

odom_msg.header.frame_id = DEFAULT_ODOM_FRAME_ID;
odom_msg.header.frame_id = this->_odom_tf;
odom_msg.child_frame_id = FRAME_ID(POSE);
odom_msg.header.stamp = t;
odom_msg.pose.pose = pose_msg.pose;
Expand All @@ -517,6 +520,10 @@ void BaseRealSenseNode::pose_callback(rs2::frame frame)
0, 0, 0, cov_twist, 0, 0,
0, 0, 0, 0, cov_twist, 0,
0, 0, 0, 0, 0, cov_twist};

// std::cout << "Sending odom with frame_id: " << odom_msg.header.frame_id
// << " and child_frame_id: " << odom_msg.child_frame_id << std::endl;

_odom_publisher->publish(odom_msg);
ROS_DEBUG("Publish %s stream", rs2_stream_to_string(frame.get_profile().stream_type()));
}
Expand Down
10 changes: 9 additions & 1 deletion realsense2_camera/src/parameters.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,13 +56,21 @@ void BaseRealSenseNode::getParameters()
_hold_back_imu_for_frames = _parameters->setParam<bool>(param_name, HOLD_BACK_IMU_FOR_FRAMES);
_parameters_names.push_back(param_name);

param_name = std::string("tf_ns");
_tf_ns = _parameters->setParam<std::string>(param_name, "tf_default");
_parameters_names.push_back(param_name);

param_name = std::string("odom_tf");
_odom_tf = _parameters->setParam<std::string>(param_name, DEFAULT_ODOM_FRAME_ID);
_parameters_names.push_back(param_name);

param_name = std::string("publish_odom_tf");
_publish_odom_tf = _parameters->setParam<bool>(param_name, PUBLISH_ODOM_TF);
_parameters_names.push_back(param_name);

param_name = std::string("base_frame_id");
_base_frame_id = _parameters->setParam<std::string>(param_name, DEFAULT_BASE_FRAME_ID);
_base_frame_id = (static_cast<std::ostringstream&&>(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str();
_base_frame_id = (static_cast<std::ostringstream&&>(std::ostringstream() << _tf_ns << (_camera_name != "" ? ("/"+_camera_name) : "") << "/" << "/base_camera_link")).str();
_parameters_names.push_back(param_name);
}

Expand Down

0 comments on commit d057f2b

Please sign in to comment.