diff --git a/CMakeLists.txt b/CMakeLists.txt index 5018202..6a249dc 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -69,6 +69,8 @@ dai_add_node_ros2(rgbd_stereo_node src/rgbd_stereo_publisher.cpp) install(DIRECTORY rviz DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY meshes DESTINATION share/${PROJECT_NAME}) install(TARGETS rgbd_stereo_node diff --git a/launch/rgbd_stereo.launch.py b/launch/rgbd_stereo.launch.py index 4b861ec..2046cd9 100644 --- a/launch/rgbd_stereo.launch.py +++ b/launch/rgbd_stereo.launch.py @@ -13,7 +13,7 @@ def generate_launch_description(): default_rviz = os.path.join(get_package_share_directory('oak_d_camera'), 'rviz', 'rgbd_stereo_pcl.rviz') - urdf_launch_dir = os.path.join(get_package_share_directory('depthai_descriptions'), 'launch') + urdf_launch_dir = os.path.join(get_package_share_directory('oak_d_camera'), 'launch') camera_model = LaunchConfiguration('camera_model', default = 'OAK-D-LITE') @@ -40,6 +40,7 @@ def generate_launch_description(): use_pointcloud = LaunchConfiguration('use_pointcloud', default = True) pc_color = LaunchConfiguration('pc_color', default = True) only_rgb = LaunchConfiguration('only_rgb', default = False) + use_base = LaunchConfiguration('use_base', default = False) declare_camera_model_cmd = DeclareLaunchArgument( @@ -151,7 +152,11 @@ def generate_launch_description(): 'only_rgb', default_value=only_rgb, description='Use only RGB image.') - + + declare_use_base_cmd = DeclareLaunchArgument( + 'use_base', + default_value=use_base, + description='Use base description.') urdf_launch = IncludeLaunchDescription( launch_description_sources.PythonLaunchDescriptionSource( @@ -165,7 +170,8 @@ def generate_launch_description(): 'cam_pos_z' : cam_pos_z, 'cam_roll' : cam_roll, 'cam_pitch' : cam_pitch, - 'cam_yaw' : cam_yaw}.items()) + 'cam_yaw' : cam_yaw, + 'use_base' : use_base}.items()) rgbd_stereo_node = launch_ros.actions.Node( @@ -275,6 +281,7 @@ def generate_launch_description(): ld.add_action(declare_use_pointcloud_cmd) ld.add_action(declare_point_cloud_color_cmd) ld.add_action(declare_only_rgb_cmd) + ld.add_action(declare_use_base_cmd) ld.add_action(rgbd_stereo_node) ld.add_action(urdf_launch) diff --git a/launch/urdf_launch.py b/launch/urdf_launch.py new file mode 100644 index 0000000..d6a6825 --- /dev/null +++ b/launch/urdf_launch.py @@ -0,0 +1,178 @@ +import os + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, OpaqueFunction +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, LaunchConfiguration +from launch_ros.actions import LoadComposableNodes, Node +from launch_ros.descriptions import ComposableNode + + +def launch_setup(context, *args, **kwargs): + bringup_dir = get_package_share_directory("oak_d_camera") + xacro_path = os.path.join(bringup_dir, "urdf", "base_descr.urdf.xacro") + + camera_model = LaunchConfiguration("camera_model", default="OAK-D") + tf_prefix = LaunchConfiguration("tf_prefix", default="oak") + base_frame = LaunchConfiguration("base_frame", default="oak-d_frame") + parent_frame = LaunchConfiguration("parent_frame", default="oak-d-base-frame") + cam_pos_x = LaunchConfiguration("cam_pos_x", default="0.0") + cam_pos_y = LaunchConfiguration("cam_pos_y", default="0.0") + cam_pos_z = LaunchConfiguration("cam_pos_z", default="0.0") + cam_roll = LaunchConfiguration("cam_roll", default="1.5708") + cam_pitch = LaunchConfiguration("cam_pitch", default="0.0") + cam_yaw = LaunchConfiguration("cam_yaw", default="1.5708") + namespace = LaunchConfiguration("namespace", default="") + rs_compat = LaunchConfiguration("rs_compat", default="false") + use_composition = LaunchConfiguration("use_composition", default="false") + use_base = LaunchConfiguration("use_base", default="false") + + name = LaunchConfiguration("tf_prefix").perform(context) + robot_description = { + "robot_description": Command( + [ + "xacro", + " ", + xacro_path, + " ", + "camera_name:=", + tf_prefix, + " ", + "camera_model:=", + camera_model, + " ", + "base_frame:=", + base_frame, + " ", + "parent_frame:=", + parent_frame, + " ", + "cam_pos_x:=", + cam_pos_x, + " ", + "cam_pos_y:=", + cam_pos_y, + " ", + "cam_pos_z:=", + cam_pos_z, + " ", + "cam_roll:=", + cam_roll, + " ", + "cam_pitch:=", + cam_pitch, + " ", + "cam_yaw:=", + cam_yaw, + " ", + "rs_compat:=", + rs_compat, + " ", + "use_base:=", + use_base, + ] + ) + } + return [ + Node( + package="robot_state_publisher", + condition=UnlessCondition(use_composition), + executable="robot_state_publisher", + name=name + "_state_publisher", + namespace=namespace, + parameters=[robot_description], + ), + LoadComposableNodes( + target_container=f"{namespace.perform(context)}/{name}_container", + condition=IfCondition(use_composition), + composable_node_descriptions=[ + ComposableNode( + package="robot_state_publisher", + plugin="robot_state_publisher::RobotStatePublisher", + name=name + "_state_publisher", + namespace=namespace, + parameters=[robot_description], + ) + ], + ), + ] + + +def generate_launch_description(): + declared_arguments = [ + DeclareLaunchArgument( + "namespace", + default_value="", + description='Specifies the namespace of the robot state publisher node. Default value will be ""', + ), + DeclareLaunchArgument( + "camera_model", + default_value="OAK-D", + description="The model of the camera. Using a wrong camera model can disable camera features. Valid models: `OAK-D, OAK-D-LITE`.", + ), + DeclareLaunchArgument( + "tf_prefix", + default_value="oak", + description="The name of the camera. It can be different from the camera model and it will be used as node `namespace`.", + ), + DeclareLaunchArgument( + "base_frame", + default_value="oak-d_frame", + description="Name of the base link.", + ), + DeclareLaunchArgument( + "parent_frame", + default_value="oak-d-base-frame", + description="Name of the parent link from other a robot TF for example that can be connected to the base of the OAK.", + ), + DeclareLaunchArgument( + "cam_pos_x", + default_value="0.0", + description="Position X of the camera with respect to the base frame.", + ), + DeclareLaunchArgument( + "cam_pos_y", + default_value="0.0", + description="Position Y of the camera with respect to the base frame.", + ), + DeclareLaunchArgument( + "cam_pos_z", + default_value="0.0", + description="Position Z of the camera with respect to the base frame.", + ), + DeclareLaunchArgument( + "cam_roll", + default_value="0.0", + description="Roll orientation of the camera with respect to the base frame.", + ), + DeclareLaunchArgument( + "cam_pitch", + default_value="0.0", + description="Pitch orientation of the camera with respect to the base frame.", + ), + DeclareLaunchArgument( + "cam_yaw", + default_value="0.0", + description="Yaw orientation of the camera with respect to the base frame.", + ), + DeclareLaunchArgument( + "use_composition", + default_value="false", + description="Use composition to start the robot_state_publisher node. Default value will be false", + ), + DeclareLaunchArgument( + "rs_compat", + default_value="false", + description="Enable RealSense compatibility mode. Default value will be false", + ), + DeclareLaunchArgument( + "use_base", + default_value="false", + description="Use base frame for the camera. Default value will be false", + ), + ] + + return LaunchDescription( + declared_arguments + [OpaqueFunction(function=launch_setup)] + ) \ No newline at end of file diff --git a/meshes/oak_dlite_base.stl b/meshes/oak_dlite_base.stl new file mode 100644 index 0000000..41ad0e3 Binary files /dev/null and b/meshes/oak_dlite_base.stl differ diff --git a/rviz/rgbd_stereo_pcl.rviz b/rviz/rgbd_stereo_pcl.rviz index e0f1cb2..cc85df1 100644 --- a/rviz/rgbd_stereo_pcl.rviz +++ b/rviz/rgbd_stereo_pcl.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /Status1 Splitter Ratio: 0.5 - Tree Height: 254 + Tree Height: 268 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -209,25 +209,25 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 5.273627281188965 + Distance: 0.44121259450912476 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -0.7700800895690918 - Y: -0.4642658233642578 - Z: 1.758968710899353 + X: -0.008556458167731762 + Y: -0.01131016667932272 + Z: 0.033811572939157486 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.6297970414161682 + Pitch: 0.2847992181777954 Target Frame: Value: Orbit (rviz_default_plugins) - Yaw: 3.1803972721099854 + Yaw: 5.420414447784424 Saved: ~ Window Geometry: Displays: @@ -235,7 +235,7 @@ Window Geometry: Height: 794 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000274000002bcfc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005d00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000013e000000cc00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb0000001400720069006700680074005f00720065006300740100000183000000b50000001700fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000fb00000012006c006500660074005f0072006500630074010000023e000000bd0000001700ffffff000000010000010f000002bcfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003f000002bc000000a900fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002d3000002bc00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000274000002cafc020000000cfb0000001200530065006c0065006300740069006f006e00000001e10000009b0000004e00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003400000145000000a900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb000000160063006f006c006f0072005f0069006d0061006700650000000256000000a20000000000000000fb0000001400720069006700680074005f0072006500630074010000017e000000ba0000001600fffffffb0000002c0072006500670069007300740065007200650064005f00640065007000740068005f0069006d006100670065000000038b000000d10000000000000000fb00000012006c006500660074005f0072006500630074010000023d000000c10000001600ffffff000000010000010f000002cafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730100000034000002ca0000009100fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000002d5000002ca00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Tool Properties: diff --git a/urdf/base_camera.urdf.xacro b/urdf/base_camera.urdf.xacro new file mode 100644 index 0000000..b76668c --- /dev/null +++ b/urdf/base_camera.urdf.xacro @@ -0,0 +1,47 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/base_descr.urdf.xacro b/urdf/base_descr.urdf.xacro new file mode 100644 index 0000000..dc26f06 --- /dev/null +++ b/urdf/base_descr.urdf.xacro @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/urdf/include/base_macro.urdf.xacro b/urdf/include/base_macro.urdf.xacro new file mode 100644 index 0000000..cb880b3 --- /dev/null +++ b/urdf/include/base_macro.urdf.xacro @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/include/depthai_macro.urdf.xacro b/urdf/include/depthai_macro.urdf.xacro new file mode 100644 index 0000000..1327ee5 --- /dev/null +++ b/urdf/include/depthai_macro.urdf.xacro @@ -0,0 +1,177 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +