diff --git a/aruco_ros/launch/single.launch.py b/aruco_ros/launch/single.launch.py index ea802522..43a8bc24 100644 --- a/aruco_ros/launch/single.launch.py +++ b/aruco_ros/launch/single.launch.py @@ -17,6 +17,7 @@ def launch_setup(context, *args, **kwargs): 'camera_frame': 'stereo_gazebo_' + eye + '_camera_optical_frame', 'marker_frame': LaunchConfiguration('marker_frame'), 'corner_refinement': LaunchConfiguration('corner_refinement'), + 'image_transport': LaunchConfiguration('image_transport'), } aruco_single = Node( @@ -65,6 +66,12 @@ def generate_launch_description(): choices=['NONE', 'HARRIS', 'LINES', 'SUBPIX'], ) + image_transport_arg = DeclareLaunchArgument( + 'image_transport', default_value='raw', + description='Image Transport. ', + choices=['raw', 'compressed'], + ) + # Create the launch description and populate ld = LaunchDescription() @@ -74,6 +81,7 @@ def generate_launch_description(): ld.add_action(marker_frame_arg) ld.add_action(reference_frame) ld.add_action(corner_refinement_arg) + ld.add_action(image_transport_arg) ld.add_action(OpaqueFunction(function=launch_setup)) diff --git a/aruco_ros/src/simple_single.cpp b/aruco_ros/src/simple_single.cpp index 520f3554..4f984d58 100644 --- a/aruco_ros/src/simple_single.cpp +++ b/aruco_ros/src/simple_single.cpp @@ -80,6 +80,7 @@ class ArucoSimple : public rclcpp::Node int marker_id; std::unique_ptr it_; + image_transport::TransportHints hints; image_transport::Subscriber image_sub; std::shared_ptr tf_listener_; @@ -88,7 +89,7 @@ class ArucoSimple : public rclcpp::Node public: ArucoSimple() - : Node("aruco_single"), cam_info_received(false) + : Node("aruco_single"), cam_info_received(false), hints(this) { } @@ -133,6 +134,7 @@ class ArucoSimple : public rclcpp::Node this->declare_parameter("image_is_rectified", true); this->declare_parameter("min_marker_size", 0.02); this->declare_parameter("detection_mode", ""); + this->declare_parameter("image_transport", "raw"); float min_marker_size; // percentage of image area this->get_parameter_or("min_marker_size", min_marker_size, 0.02); @@ -152,7 +154,7 @@ class ArucoSimple : public rclcpp::Node this->get_logger(), "Marker size min: " << min_marker_size << " of image area"); RCLCPP_INFO_STREAM(this->get_logger(), "Detection mode: " << detection_mode); - image_sub = it_->subscribe("/image", 1, &ArucoSimple::image_callback, this); + image_sub = it_->subscribe("/image", 1, &ArucoSimple::image_callback, this, &hints); cam_info_sub = this->create_subscription( "/camera_info", 1, std::bind( &ArucoSimple::cam_info_callback, this,