diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json deleted file mode 100644 index d022d76..0000000 --- a/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,21 +0,0 @@ -{ - "configurations": [ - { - "browse": { - "databaseFilename": "", - "limitSymbolsToIncludedHeaders": true - }, - "includePath": [ - "/opt/ros/kinetic/include", - "/src/visionmodule_ros/include", - "/usr/include" - ], - "name": "Linux", - "intelliSenseMode": "gcc-x64", - "compilerPath": "/usr/bin/gcc", - "cStandard": "c11", - "cppStandard": "c++17" - } - ], - "version": 4 -} \ No newline at end of file diff --git a/include/ros_kortex_vision/vision.h b/include/ros_kortex_vision/vision.h index 83e935e..fd4ef20 100644 --- a/include/ros_kortex_vision/vision.h +++ b/include/ros_kortex_vision/vision.h @@ -75,4 +75,4 @@ class Vision bool use_gst_timestamps_; bool is_first_initialize_; }; -} +} // namespace ros_kortex_vision diff --git a/launch/kinova_vision.launch.py b/launch/kinova_vision.launch.py index d64287e..c648546 100644 --- a/launch/kinova_vision.launch.py +++ b/launch/kinova_vision.launch.py @@ -9,28 +9,89 @@ configurable_parameters = [ {"name": "device", "default": "192.168.1.10", "description": "Device IPv4 address"}, - {"name": "camera", "default": "camera", "description": "'camera' should uniquely identify the device. All topics are pushed down into the 'camera' namespace."}, - {"name": "camera_link_frame_id", "default": "camera_link", "description": "Camera link frame identifier"}, - {"name": "color_frame_id", "default": "camera_color_frame", "description": "Color camera frame identifier"}, - {"name": "depth_frame_id", "default": "camera_depth_frame", "description": "Depth camera frame identifier"}, - {"name": "color_camera_info_url", "default": "", "description": "URL of custom calibration file for color camera. See camera_info_manager docs for calibration URL details"}, - {"name": "depth_camera_info_url", "default": "", "description": "URL of custom calibration file for depth camera. See camera_info_manager docs for calibration URL details"}, - {"name": "depth_rtsp_element_config", "default": "depth latency=30", "description": "RTSP element configuration for depth stream"}, - {"name": "depth_rtp_depay_element_config", "default": "rtpgstdepay", "description": "RTP element configuration for depth stream"}, - {"name": "color_rtsp_element_config", "default": "color latency=30", "description": "RTSP element configuration for color stream"}, - {"name": "color_rtp_depay_element_config", "default": "rtph264depay", "description": "RTP element configuration for color stream"}, - {"name": "launch_color", "default": "true", "description": "Launch the color image node"}, - {"name": "launch_depth", "default": "true", "description": "Launch the depth image node"}, - {"name": "depth_registration", "default": "false", "description": "Hardware depth registration"}, + { + "name": "camera", + "default": "camera", + "description": "'camera' should uniquely identify the device. All topics are pushed down into the 'camera' namespace.", + }, + { + "name": "camera_link_frame_id", + "default": "camera_link", + "description": "Camera link frame identifier", + }, + { + "name": "color_frame_id", + "default": "camera_color_frame", + "description": "Color camera frame identifier", + }, + { + "name": "depth_frame_id", + "default": "camera_depth_frame", + "description": "Depth camera frame identifier", + }, + { + "name": "color_camera_info_url", + "default": "", + "description": "URL of custom calibration file for color camera. See camera_info_manager docs for calibration URL details", + }, + { + "name": "depth_camera_info_url", + "default": "", + "description": "URL of custom calibration file for depth camera. See camera_info_manager docs for calibration URL details", + }, + { + "name": "depth_rtsp_element_config", + "default": "depth latency=30", + "description": "RTSP element configuration for depth stream", + }, + { + "name": "depth_rtp_depay_element_config", + "default": "rtpgstdepay", + "description": "RTP element configuration for depth stream", + }, + { + "name": "color_rtsp_element_config", + "default": "color latency=30", + "description": "RTSP element configuration for color stream", + }, + { + "name": "color_rtp_depay_element_config", + "default": "rtph264depay", + "description": "RTP element configuration for color stream", + }, + { + "name": "launch_color", + "default": "true", + "description": "Launch the color image node", + }, + { + "name": "launch_depth", + "default": "true", + "description": "Launch the depth image node", + }, + { + "name": "depth_registration", + "default": "false", + "description": "Hardware depth registration", + }, ] def declare_configurable_parameters(): - return [DeclareLaunchArgument(param["name"], default_value=param["default"], description=param["description"]) for param in configurable_parameters] + return [ + DeclareLaunchArgument( + param["name"], + default_value=param["default"], + description=param["description"], + ) + for param in configurable_parameters + ] def set_configurable_parameters(parameters): - return dict([(param["name"], LaunchConfiguration(param["name"])) for param in parameters]) + return dict( + [(param["name"], LaunchConfiguration(param["name"])) for param in parameters] + ) def yaml_to_dict(path_to_yaml): @@ -43,18 +104,29 @@ def launch_setup(context, *args, **kwargs): # Depth Node Configuration depth_node = Node( package="kinova_vision", - namespace=LaunchConfiguration('camera'), + namespace=LaunchConfiguration("camera"), executable="kinova_vision_node", name="kinova_vision_depth", output="both", - parameters=[{ - "camera_type": "depth", - "camera_name": "depth", - "camera_info_url_default": "package://kinova_vision/launch/calibration/default_depth_calib_%ux%u.ini", - "camera_info_url_user": LaunchConfiguration("depth_camera_info_url").perform(context), - "stream_config": "rtspsrc location=rtsp://" + LaunchConfiguration("device").perform(context) + "/" + LaunchConfiguration("depth_rtsp_element_config").perform(context) + " ! " + LaunchConfiguration("depth_rtp_depay_element_config").perform(context), - "frame_id": LaunchConfiguration("depth_frame_id").perform(context), - }], + parameters=[ + { + "camera_type": "depth", + "camera_name": "depth", + "camera_info_url_default": "package://kinova_vision/launch/calibration/default_depth_calib_%ux%u.ini", + "camera_info_url_user": LaunchConfiguration( + "depth_camera_info_url" + ).perform(context), + "stream_config": "rtspsrc location=rtsp://" + + LaunchConfiguration("device").perform(context) + + "/" + + LaunchConfiguration("depth_rtsp_element_config").perform(context) + + " ! " + + LaunchConfiguration("depth_rtp_depay_element_config").perform( + context + ), + "frame_id": LaunchConfiguration("depth_frame_id").perform(context), + } + ], remappings=[ ("camera_info", "depth/camera_info"), ("image_raw", "depth/image_raw"), @@ -62,24 +134,34 @@ def launch_setup(context, *args, **kwargs): ("image_raw/compressedDepth", "depth/image_raw/compressedDepth"), ("image_raw/theora", "depth/image_raw/theora"), ], - condition=IfCondition(LaunchConfiguration('launch_depth')), + condition=IfCondition(LaunchConfiguration("launch_depth")), ) # Color node configuration color_node = Node( package="kinova_vision", - namespace=LaunchConfiguration('camera'), + namespace=LaunchConfiguration("camera"), executable="kinova_vision_node", name="kinova_vision_color", output="both", - parameters=[{ - "camera_type": "color", - "camera_name": "color", - "camera_info_url_default": "package://kinova_vision/launch/calibration/default_color_calib_%ux%u.ini", - "camera_info_url_user": LaunchConfiguration("color_camera_info_url").perform(context), - "stream_config": "rtspsrc location=rtsp://" + LaunchConfiguration("device").perform(context) + "/" + LaunchConfiguration("color_rtsp_element_config").perform(context) + " ! " + LaunchConfiguration("color_rtp_depay_element_config").perform(context) + " ! avdec_h264 ! videoconvert", - "frame_id": LaunchConfiguration("color_frame_id").perform(context), - }], + parameters=[ + { + "camera_type": "color", + "camera_name": "color", + "camera_info_url_default": "package://kinova_vision/launch/calibration/default_color_calib_%ux%u.ini", + "camera_info_url_user": LaunchConfiguration( + "color_camera_info_url" + ).perform(context), + "stream_config": "rtspsrc location=rtsp://" + + LaunchConfiguration("device").perform(context) + + "/" + + LaunchConfiguration("color_rtsp_element_config").perform(context) + + " ! " + + LaunchConfiguration("color_rtp_depay_element_config").perform(context) + + " ! avdec_h264 ! videoconvert", + "frame_id": LaunchConfiguration("color_frame_id").perform(context), + } + ], remappings=[ ("camera_info", "color/camera_info"), ("image_raw", "color/image_raw"), @@ -87,13 +169,13 @@ def launch_setup(context, *args, **kwargs): ("image_raw/compressedDepth", "color/image_raw/compressedDepth"), ("image_raw/theora", "color/image_raw/theora"), ], - condition=IfCondition(LaunchConfiguration('launch_color')), + condition=IfCondition(LaunchConfiguration("launch_color")), ) # Conditionally launch tools to publish registered depth images depth_image_proc = ComposableNodeContainer( name="registered_depth_images", - namespace=LaunchConfiguration('camera'), + namespace=LaunchConfiguration("camera"), package="rclcpp_components", executable="component_container", composable_node_descriptions=[ @@ -101,7 +183,7 @@ def launch_setup(context, *args, **kwargs): package="depth_image_proc", plugin="depth_image_proc::RegisterNode", name="register_node", - namespace=LaunchConfiguration('camera'), + namespace=LaunchConfiguration("camera"), remappings=[ ("rgb/camera_info", "color/camera_info"), ("depth/camera_info", "depth/camera_info"), @@ -112,7 +194,7 @@ def launch_setup(context, *args, **kwargs): package="depth_image_proc", plugin="depth_image_proc::PointCloudXyzrgbNode", name=("point_cloud_xyzrgb"), - namespace=LaunchConfiguration('camera'), + namespace=LaunchConfiguration("camera"), remappings=[ ("rgb/camera_info", "color/camera_info"), ("depth/camera_info", "depth/camera_info"), @@ -123,38 +205,58 @@ def launch_setup(context, *args, **kwargs): ), ], output="both", - condition=IfCondition(LaunchConfiguration('depth_registration')), + condition=IfCondition(LaunchConfiguration("depth_registration")), ) # Static Transformation Publishers camera_depth_tf_publisher = Node( - package='tf2_ros', - namespace=LaunchConfiguration('camera'), - executable='static_transform_publisher', - name='camera_depth_tf_publisher', + package="tf2_ros", + namespace=LaunchConfiguration("camera"), + executable="static_transform_publisher", + name="camera_depth_tf_publisher", output="both", - arguments=['-0.0195', '-0.005', '0', '0', '0', '0', - LaunchConfiguration('camera_link_frame_id'), - LaunchConfiguration('depth_frame_id')], - condition=IfCondition(LaunchConfiguration('launch_depth')), + arguments=[ + "-0.0195", + "-0.005", + "0", + "0", + "0", + "0", + LaunchConfiguration("camera_link_frame_id"), + LaunchConfiguration("depth_frame_id"), + ], + condition=IfCondition(LaunchConfiguration("launch_depth")), ) camera_color_tf_publisher = Node( - package='tf2_ros', - namespace=LaunchConfiguration('camera'), - executable='static_transform_publisher', - name='camera_color_tf_publisher', + package="tf2_ros", + namespace=LaunchConfiguration("camera"), + executable="static_transform_publisher", + name="camera_color_tf_publisher", output="both", - arguments=['0', '0', '0', '0', '0', '0', - LaunchConfiguration('camera_link_frame_id'), - LaunchConfiguration('color_frame_id')], - condition=IfCondition(LaunchConfiguration('launch_color')), + arguments=[ + "0", + "0", + "0", + "0", + "0", + "0", + LaunchConfiguration("camera_link_frame_id"), + LaunchConfiguration("color_frame_id"), + ], + condition=IfCondition(LaunchConfiguration("launch_color")), ) - return [depth_node, color_node, camera_depth_tf_publisher, camera_color_tf_publisher, depth_image_proc] + return [ + depth_node, + color_node, + camera_depth_tf_publisher, + camera_color_tf_publisher, + depth_image_proc, + ] def generate_launch_description(): - return LaunchDescription(declare_configurable_parameters() + [ - OpaqueFunction(function=launch_setup) - ]) + return LaunchDescription( + declare_configurable_parameters() + [OpaqueFunction(function=launch_setup)] + )