diff --git a/realsense2_camera/include/base_realsense_node.h b/realsense2_camera/include/base_realsense_node.h index 27024b14f8..4c8d0935c9 100755 --- a/realsense2_camera/include/base_realsense_node.h +++ b/realsense2_camera/include/base_realsense_node.h @@ -41,9 +41,13 @@ using realsense2_camera_msgs::msg::Extrinsics; using realsense2_camera_msgs::msg::IMUInfo; -#define FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_frame")).str() -#define OPTICAL_FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_optical_frame")).str() -#define ALIGNED_DEPTH_TO_FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << "aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str() +// #define FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_frame")).str() +// #define OPTICAL_FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << STREAM_NAME(sip) << "_optical_frame")).str() +// #define ALIGNED_DEPTH_TO_FRAME_ID(sip) (static_cast(std::ostringstream() << _camera_name << "_" << "aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str() + +#define FRAME_ID(sip) (static_cast(std::ostringstream() << _tf_ns << "/" << _camera_name << (_camera_name != "" ? "/" : "") << STREAM_NAME(sip) << "_frame")).str() +#define OPTICAL_FRAME_ID(sip) (static_cast(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() << _tf_ns << "/" << _camera_name << (_camera_name != "" ? "/" : "") << "aligned_depth_to_" << STREAM_NAME(sip) << "_frame")).str() namespace realsense2_camera { @@ -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; diff --git a/realsense2_camera/include/constants.h b/realsense2_camera/include/constants.h index f640bab854..3cc8300bee 100644 --- a/realsense2_camera/include/constants.h +++ b/realsense2_camera/include/constants.h @@ -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 = ""; diff --git a/realsense2_camera/launch/rs_launch.py b/realsense2_camera/launch/rs_launch.py index 0bc2ec3e6d..8ff0306d25 100644 --- a/realsense2_camera/launch/rs_launch.py +++ b/realsense2_camera/launch/rs_launch.py @@ -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'}, @@ -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): @@ -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) @@ -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) @@ -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) ], @@ -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")]) diff --git a/realsense2_camera/package.xml b/realsense2_camera/package.xml index b5cedbe649..ff2bcd5c54 100644 --- a/realsense2_camera/package.xml +++ b/realsense2_camera/package.xml @@ -17,7 +17,7 @@ builtin_interfaces cv_bridge image_transport - librealsense2 + rclcpp rclcpp_components realsense2_camera_msgs diff --git a/realsense2_camera/src/base_realsense_node.cpp b/realsense2_camera/src/base_realsense_node.cpp index 81c318099d..6d28b515d4 100755 --- a/realsense2_camera/src/base_realsense_node.cpp +++ b/realsense2_camera/src/base_realsense_node.cpp @@ -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; @@ -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()) @@ -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; @@ -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())); } diff --git a/realsense2_camera/src/parameters.cpp b/realsense2_camera/src/parameters.cpp index fd460d9888..870d3c4545 100644 --- a/realsense2_camera/src/parameters.cpp +++ b/realsense2_camera/src/parameters.cpp @@ -56,13 +56,21 @@ void BaseRealSenseNode::getParameters() _hold_back_imu_for_frames = _parameters->setParam(param_name, HOLD_BACK_IMU_FOR_FRAMES); _parameters_names.push_back(param_name); + param_name = std::string("tf_ns"); + _tf_ns = _parameters->setParam(param_name, "tf_default"); + _parameters_names.push_back(param_name); + + param_name = std::string("odom_tf"); + _odom_tf = _parameters->setParam(param_name, DEFAULT_ODOM_FRAME_ID); + _parameters_names.push_back(param_name); + param_name = std::string("publish_odom_tf"); _publish_odom_tf = _parameters->setParam(param_name, PUBLISH_ODOM_TF); _parameters_names.push_back(param_name); param_name = std::string("base_frame_id"); _base_frame_id = _parameters->setParam(param_name, DEFAULT_BASE_FRAME_ID); - _base_frame_id = (static_cast(std::ostringstream() << _camera_name << "_" << _base_frame_id)).str(); + _base_frame_id = (static_cast(std::ostringstream() << _tf_ns << (_camera_name != "" ? ("/"+_camera_name) : "") << "/" << "/base_camera_link")).str(); _parameters_names.push_back(param_name); }