From a0bc503e3263745f2e747115a3846c758b4d88cd Mon Sep 17 00:00:00 2001 From: Your Name Date: Thu, 29 Jan 2026 11:33:17 +0800 Subject: [PATCH] orbbec dual camera --- .../include/ethercat_driver/zer_config.hpp | 4 ++-- ecrt_dev_dual/launch/dual_arm.launch.py | 7 +++++-- .../launch/multi_camera.launch.py | 4 ++-- .../launch/multi_camera_synced.launch.py | 4 ++-- .../launch/orbbec_multicamera.launch.py | 18 +++++++++--------- 5 files changed, 20 insertions(+), 17 deletions(-) diff --git a/ecrt_dev_dual/include/ethercat_driver/zer_config.hpp b/ecrt_dev_dual/include/ethercat_driver/zer_config.hpp index a4e81d7..47e49e1 100644 --- a/ecrt_dev_dual/include/ethercat_driver/zer_config.hpp +++ b/ecrt_dev_dual/include/ethercat_driver/zer_config.hpp @@ -22,7 +22,7 @@ typedef struct { static zer_offset_t zer_offsets[NUM_SLAVES]; static double rated_currents[NUM_SLAVES]={12.5,12.5,6.3,5.4,5.4,5.4,12.5,12.5,6.3,5.4,5.4,5.4}; -static double pos_offsets[NUM_SLAVES]={248214.0,358098.0,251704.0,240977.0,280113.0,54646.0, 211980.0,262157.0,281128.0,270017.0,263697.0,275363.0}; +static double pos_offsets[NUM_SLAVES]={248214.0,358098.0,251704.0,240977.0,280113.0,54646.0, 211980.0,262157.0,281128.0,270017.0,268839.0,275363.0}; //static double pos_offsets[NUM_SLAVES]={-13930.0, 95954.0, -10440.0, -21167.0, 17969.0, -207498.0, -50164.0, 13.0, 18984.0, 7873.0, 1553.0, 13219.0}; constexpr double rad_to_count= 524288.0/(2*M_PI); constexpr double count_to_rad=2*M_PI/524288.0; @@ -113,4 +113,4 @@ static ec_sync_info_t zer_device_syncs[] = { {2, EC_DIR_OUTPUT, 1, zer_device_pdos + 0, EC_WD_ENABLE}, {3, EC_DIR_INPUT, 1, zer_device_pdos + 1, EC_WD_DISABLE}, {0xff} -}; \ No newline at end of file +}; diff --git a/ecrt_dev_dual/launch/dual_arm.launch.py b/ecrt_dev_dual/launch/dual_arm.launch.py index 3cb1712..c17db54 100644 --- a/ecrt_dev_dual/launch/dual_arm.launch.py +++ b/ecrt_dev_dual/launch/dual_arm.launch.py @@ -25,7 +25,10 @@ def generate_launch_description(): PathJoinSubstitution([ FindPackageShare("ecrt_driver"), "config", "dual_arm_controllers.yaml" - ])], + ]),{"lock_memory": True, + "thread_priority": 99, + "cpu_affinity": 6} + ], output="both")) ld.add_action(Node( package="robot_state_publisher", @@ -49,4 +52,4 @@ def generate_launch_description(): ld.add_action(right) ld.add_action(l_gpio) ld.add_action(r_gpio) - return ld \ No newline at end of file + return ld diff --git a/orbbec/orbbec_camera/launch/multi_camera.launch.py b/orbbec/orbbec_camera/launch/multi_camera.launch.py index 821af5b..5d89758 100644 --- a/orbbec/orbbec_camera/launch/multi_camera.launch.py +++ b/orbbec/orbbec_camera/launch/multi_camera.launch.py @@ -16,7 +16,7 @@ def generate_launch_description(): ), launch_arguments={ 'camera_name': 'camera_01', - 'usb_port': '1-5', + 'usb_port': '2-3.3', 'device_num': '2', 'sync_mode': 'standalone', 'enable_left_ir': 'true', @@ -30,7 +30,7 @@ def generate_launch_description(): ), launch_arguments={ 'camera_name': 'camera_02', - 'usb_port': '2-4', + 'usb_port': '2-3.4', 'device_num': '2', 'sync_mode': 'standalone', 'enable_left_ir': 'true', diff --git a/orbbec/orbbec_camera/launch/multi_camera_synced.launch.py b/orbbec/orbbec_camera/launch/multi_camera_synced.launch.py index 4c7a8e5..6809144 100644 --- a/orbbec/orbbec_camera/launch/multi_camera_synced.launch.py +++ b/orbbec/orbbec_camera/launch/multi_camera_synced.launch.py @@ -20,7 +20,7 @@ def generate_launch_description(): ), launch_arguments={ "camera_name": "camera_01", - "usb_port": "2-1", + "usb_port": "2-3.3", "device_num": "2", "sync_mode": "primary", "config_file_path": config_file_path, @@ -34,7 +34,7 @@ def generate_launch_description(): ), launch_arguments={ "camera_name": "camera_02", - "usb_port": "2-3", + "usb_port": "2-3.4", "device_num": "2", "sync_mode": "secondary_synced", "config_file_path": secondary_config_file_path, diff --git a/orbbec/orbbec_camera/launch/orbbec_multicamera.launch.py b/orbbec/orbbec_camera/launch/orbbec_multicamera.launch.py index 3018201..57d3533 100644 --- a/orbbec/orbbec_camera/launch/orbbec_multicamera.launch.py +++ b/orbbec/orbbec_camera/launch/orbbec_multicamera.launch.py @@ -19,8 +19,8 @@ def generate_launch_description(): 'camera_model': 'gemini330_series', 'attach_component_container_enable': 'true', 'camera_name': 'G330_0', - 'usb_port': '2-1', - 'device_num': '4', + 'usb_port': '2-3.3', + 'device_num': '2', # 'sync_mode': 'primary', }.items() ) @@ -33,13 +33,13 @@ def generate_launch_description(): 'camera_model': 'gemini330_series', 'attach_component_container_enable': 'false', 'camera_name': 'G330_1', - 'usb_port': '2-4', - 'device_num': '4', + 'usb_port': '2-3.4', + 'device_num': '2', # 'sync_mode': 'secondary_synced', # 'trigger_out_enabled': 'false', }.items() ) - + ''' G330_2 = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(launch_file_dir, 'orbbec_camera.launch.py') @@ -69,18 +69,18 @@ def generate_launch_description(): # 'trigger_out_enabled': 'false', }.items() ) - + ''' # If you need more cameras, just add more launch_include here, and change the usb_port and device_num # Launch description ld = LaunchDescription( [ TimerAction(period=0.0, actions=[GroupAction([G330_1])]), - TimerAction(period=2.0, actions=[GroupAction([G330_2])]), - TimerAction(period=4.0, actions=[GroupAction([G330_3])]), + # TimerAction(period=2.0, actions=[GroupAction([G330_2])]), + # TimerAction(period=4.0, actions=[GroupAction([G330_3])]), TimerAction(period=6.0, actions=[GroupAction([G330_0])]), # The primary camera should be launched at last ] ) - return ld \ No newline at end of file + return ld