Skip to content

Commit 131e477

Browse files
luis-cameroluis-camero
authored
Feature: Kinova Jazzy Support (#268)
* Add Kinova vision node to manipulator launch file * Kinova camera rate from string to double * Use node name to namespace Kinova camera topics * Add Kinova Pointcloud ComposableNodeContainer * Fix Kinova processing container remappings * Remove unnecessary lines * Rename depth registered node * Update depth registered node in pointcloud generator * Add missing remap to pointcloud processor * Lint
1 parent 513639b commit 131e477

File tree

1 file changed

+149
-1
lines changed
  • clearpath_generator_robot/clearpath_generator_robot/launch

1 file changed

+149
-1
lines changed

‎clearpath_generator_robot/clearpath_generator_robot/launch/generator.py‎

Lines changed: 149 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,14 @@
3434
import os
3535

3636
from clearpath_config.common.types.platform import Platform
37-
from clearpath_config.manipulators.types.arms import Franka, UniversalRobots
37+
from clearpath_config.manipulators.types.arms import (
38+
BaseKinova,
39+
Franka,
40+
KinovaGen3Dof6,
41+
KinovaGen3Dof7,
42+
KinovaGen3Lite,
43+
UniversalRobots
44+
)
3845
from clearpath_config.manipulators.types.grippers import FrankaGripper
3946
from clearpath_config.platform.battery import BatteryConfig
4047
from clearpath_generator_common.common import LaunchFile, Package
@@ -565,6 +572,147 @@ def generate_manipulators(self) -> None:
565572
]
566573
)
567574
manipulator_service_launch_writer.add_node(node)
575+
# Kinova Vision
576+
if (arm.MANIPULATOR_MODEL == KinovaGen3Dof6.MANIPULATOR_MODEL or
577+
arm.MANIPULATOR_MODEL == KinovaGen3Dof7.MANIPULATOR_MODEL or
578+
arm.MANIPULATOR_MODEL == KinovaGen3Lite.MANIPULATOR_MODEL):
579+
if (arm.get_urdf_parameters().get(BaseKinova.VISION, False)):
580+
depth_node_parameters = {
581+
'camera_type': 'depth',
582+
'camera_name': 'depth',
583+
'camera_info_url_default':
584+
'package://kinova_vision/launch/calibration/default_depth_calib_%ux%u.ini',
585+
'camera_info_url_user': '',
586+
'stream_config': 'rtspsrc location=rtsp://'
587+
+ arm.ip
588+
+ '/depth latency=30'
589+
+ ' ! '
590+
+ 'rtpgstdepay',
591+
'frame_id': f'{arm.name}_camera_depth_frame',
592+
'max_pub_rate': 30.0,
593+
}
594+
depth_node = LaunchFile.Node(
595+
name=f'{arm.name}_depth_camera',
596+
package='kinova_vision',
597+
executable='kinova_vision_node',
598+
namespace=f'{self.namespace}/manipulators',
599+
parameters=[depth_node_parameters],
600+
remappings=[
601+
('camera_info', '~/camera_info'),
602+
('image_raw', '~/image_raw'),
603+
('image_raw/compressed', '~/image_raw/compressed'),
604+
('image_raw/compressedDepth', '~/image_raw/compressedDepth'),
605+
('image_raw/theora', '~/image_raw/theora'),
606+
('image_raw/ffmpeg', '~/image_raw/ffmpeg'),
607+
('image_raw/zstd', '~/image_raw/zstd'),
608+
]
609+
)
610+
color_node_parameters = {
611+
'camera_type': 'color',
612+
'camera_name': 'color',
613+
'camera_info_url_default':
614+
'package://kinova_vision/launch/calibration/default_color_calib_%ux%u.ini',
615+
'camera_info_url_user': '',
616+
'stream_config': 'rtspsrc location=rtsp://'
617+
+ arm.ip
618+
+ '/color latency=30'
619+
+ ' ! rtph264depay ! avdec_h264 ! videoconvert',
620+
'frame_id': f'{arm.name}_camera_color_frame',
621+
'max_pub_rate': 30.0,
622+
}
623+
color_node = LaunchFile.Node(
624+
name=f'{arm.name}_color_camera',
625+
package='kinova_vision',
626+
executable='kinova_vision_node',
627+
namespace=f'{self.namespace}/manipulators',
628+
parameters=[color_node_parameters],
629+
remappings=[
630+
('camera_info', '~/camera_info'),
631+
('image_raw', '~/image_raw'),
632+
('image_raw/compressed', '~/image_raw/compressed'),
633+
('image_raw/compressedDepth', '~/image_raw/compressedDepth'),
634+
('image_raw/theora', '~/image_raw/theora'),
635+
('image_raw/ffmpeg', '~/image_raw/ffmpeg'),
636+
('image_raw/zstd', '~/image_raw/zstd'),
637+
]
638+
)
639+
640+
pointcloud_node = LaunchFile.ComposableNodeContainer(
641+
name=f'{arm.name}_depth_proc_container',
642+
namespace=f'{self.namespace}/manipulators',
643+
remappings=[
644+
('/tf', f'/{self.namespace}/tf'),
645+
('/tf_static', f'/{self.namespace}/tf_static')
646+
],
647+
composable_node_descriptions=[
648+
LaunchFile.ComposableNode(
649+
name=f'{arm.name}_depth_upsampled',
650+
package='depth_image_proc',
651+
plugin='depth_image_proc::RegisterNode',
652+
namespace=f'{self.namespace}/manipulators',
653+
parameters=[{'fill_upsampling_holes': True}],
654+
remappings=[
655+
('rgb/camera_info',
656+
f'{arm.name}_color_camera/camera_info'),
657+
('depth/camera_info',
658+
f'{arm.name}_depth_camera/camera_info'),
659+
('depth/image_rect',
660+
f'{arm.name}_depth_camera/image_raw'),
661+
('depth_registered/camera_info',
662+
'~/camera_info'),
663+
('depth_registered/image_rect',
664+
'~/image_rect'),
665+
('depth_registered/image_rect/compressed',
666+
'~/image_rect/compressed'),
667+
('depth_registered/image_rect/compressedDepth',
668+
'~/image_rect/compressedDepth'),
669+
('depth_registered/image_rect/ffmpeg',
670+
'~/image_rect/ffmpeg'),
671+
('depth_registered/image_rect/theora',
672+
'~/image_rect/theora'),
673+
('depth_registered/image_rect/zstd',
674+
'~/image_rect/zstd'),
675+
]
676+
),
677+
LaunchFile.ComposableNode(
678+
name=f'{arm.name}_pointcloud_node',
679+
package='depth_image_proc',
680+
plugin='depth_image_proc::PointCloudXyzrgbNode',
681+
namespace=f'{self.namespace}/manipulators',
682+
remappings=[
683+
('depth_registered/camera_info',
684+
f'{arm.name}_depth_upsampled/camera_info'),
685+
('depth_registered/image_rect',
686+
f'{arm.name}_depth_upsampled/image_rect'),
687+
('depth_registered/image_rect/compressed',
688+
f'{arm.name}_depth_upsampled/image_rect/compressed'),
689+
('depth_registered/image_rect/compressedDepth',
690+
f'{arm.name}_depth_upsampled/image_rect/compressedDepth'),
691+
('depth_registered/image_rect/ffmpeg',
692+
f'{arm.name}_depth_upsampled/image_rect/ffmpeg'),
693+
('depth_registered/image_rect/theora',
694+
f'{arm.name}_depth_upsampled/image_rect/theora'),
695+
('depth_registered/image_rect/zstd',
696+
f'{arm.name}_depth_upsampled/image_rect/zstd'),
697+
('rgb/camera_info',
698+
f'{arm.name}_color_camera/camera_info'),
699+
('depth/camera_info',
700+
f'{arm.name}_depth_camera/camera_info'),
701+
('rgb/image_rect_color',
702+
f'{arm.name}_color_camera/image_raw'),
703+
('depth/image_rect',
704+
f'{arm.name}_depth_camera/image_raw'),
705+
('points',
706+
f'{arm.name}_depth_camera/color/points'),
707+
]
708+
)
709+
]
710+
)
711+
712+
manipulator_service_launch_writer.add_node(depth_node)
713+
manipulator_service_launch_writer.add_node(color_node)
714+
manipulator_service_launch_writer.add(pointcloud_node)
715+
568716
if self.clearpath_config.manipulators.get_all_manipulators():
569717
manipulator_service_launch_writer.add(self.manipulators_launch_file)
570718
manipulator_service_launch_writer.generate_file()

0 commit comments

Comments
 (0)