Error in prepare_command_mode_switch when running robotiq gripper controller

[peterdavidfagan]

I am encountering an error from the hardware interface with the latest version for franka_ros2, the error arises when activating controllers for a gripper (robotiq) and the panda arm under the one controller manager node.

Adding this error for reference in case others also encounter this issue, I will post my resolution below. I started by trying to run separate control_manager nodes for the arm and gripper but I think I also found an issue with this given the current ros2_control implementation on Humble.

Update: namespacing with ROS 2 control works well and avoids this issue.

root@franka-control:~/panda_ws# ros2 launch panda_control_demos control_server_debug.launch.py robot_ip:=192.168.106.99 use_fake_hardware:=false
[INFO] [launch]: All log files can be found below /root/.ros/log/2024-04-03-17-17-40-409065-franka-control-35799
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [joint_state_publisher-1]: process started with pid [35800]
[INFO] [robot_state_publisher-2]: process started with pid [35802]
[INFO] [ros2_control_node-3]: process started with pid [35804]
[INFO] [ros2 run controller_manager spawner panda_jtc_controller-4]: process started with pid [35806]
[INFO] [ros2 run controller_manager spawner panda_state_broadcaster-5]: process started with pid [35808]
[INFO] [ros2 run controller_manager spawner robotiq_state_broadcaster-6]: process started with pid [35810]
[INFO] [ros2 run controller_manager spawner robotiq_gripper_controller-7]: process started with pid [35812]
[robot_state_publisher-2] [INFO] [1712164660.608402668] [panda_state_publisher]: got segment panda_link0
[robot_state_publisher-2] [INFO] [1712164660.608460540] [panda_state_publisher]: got segment panda_link1
[robot_state_publisher-2] [INFO] [1712164660.608466522] [panda_state_publisher]: got segment panda_link2
[robot_state_publisher-2] [INFO] [1712164660.608469562] [panda_state_publisher]: got segment panda_link3
[robot_state_publisher-2] [INFO] [1712164660.608472392] [panda_state_publisher]: got segment panda_link4
[robot_state_publisher-2] [INFO] [1712164660.608475100] [panda_state_publisher]: got segment panda_link5
[robot_state_publisher-2] [INFO] [1712164660.608477667] [panda_state_publisher]: got segment panda_link6
[robot_state_publisher-2] [INFO] [1712164660.608480275] [panda_state_publisher]: got segment panda_link7
[robot_state_publisher-2] [INFO] [1712164660.608483043] [panda_state_publisher]: got segment panda_link8
[robot_state_publisher-2] [INFO] [1712164660.608485668] [panda_state_publisher]: got segment robotiq_85_base_link
[robot_state_publisher-2] [INFO] [1712164660.608488616] [panda_state_publisher]: got segment robotiq_85_left_finger_link
[robot_state_publisher-2] [INFO] [1712164660.608491366] [panda_state_publisher]: got segment robotiq_85_left_finger_tip_link
[robot_state_publisher-2] [INFO] [1712164660.608494288] [panda_state_publisher]: got segment robotiq_85_left_inner_knuckle_link
[robot_state_publisher-2] [INFO] [1712164660.608497094] [panda_state_publisher]: got segment robotiq_85_left_knuckle_link
[robot_state_publisher-2] [INFO] [1712164660.608499798] [panda_state_publisher]: got segment robotiq_85_right_finger_link
[robot_state_publisher-2] [INFO] [1712164660.608503095] [panda_state_publisher]: got segment robotiq_85_right_finger_tip_link
[robot_state_publisher-2] [INFO] [1712164660.608507831] [panda_state_publisher]: got segment robotiq_85_right_inner_knuckle_link
[robot_state_publisher-2] [INFO] [1712164660.608513546] [panda_state_publisher]: got segment robotiq_85_right_knuckle_link
[ros2_control_node-3] [WARN] [1712164660.624893052] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '~/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-3] [INFO] [1712164660.625457791] [resource_manager]: Loading hardware 'RobotiqGripperHardwareInterface' 
[ros2_control_node-3] [INFO] [1712164660.626245168] [resource_manager]: Initialize hardware 'RobotiqGripperHardwareInterface' 
[ros2_control_node-3] [INFO] [1712164660.626360955] [DefaultDriverFactory]: Reading slave_address...
[ros2_control_node-3] [INFO] [1712164660.626367766] [DefaultDriverFactory]: slave_address: 9
[ros2_control_node-3] [INFO] [1712164660.626369915] [DefaultDriverFactory]: Reading gripper_speed_multiplier...
[ros2_control_node-3] [INFO] [1712164660.626375080] [DefaultDriverFactory]: gripper_speed_multiplier: 1.000000s
[ros2_control_node-3] [INFO] [1712164660.626379117] [DefaultDriverFactory]: Reading gripper_force_multiplier...
[ros2_control_node-3] [INFO] [1712164660.626381252] [DefaultDriverFactory]: gripper_force_multiplier: 0.500000s
[ros2_control_node-3] [INFO] [1712164660.626385329] [DefaultSerialFactory]: Reading COM_port...
[ros2_control_node-3] [INFO] [1712164660.626388065] [DefaultSerialFactory]: COM_port: /dev/ttyUSB0
[ros2_control_node-3] [INFO] [1712164660.626389579] [DefaultSerialFactory]: Reading baudrate...
[ros2_control_node-3] [INFO] [1712164660.626391030] [DefaultSerialFactory]: baudrate: 115200bps
[ros2_control_node-3] [INFO] [1712164660.626392457] [DefaultSerialFactory]: Reading timeout...
[ros2_control_node-3] [INFO] [1712164660.626393926] [DefaultSerialFactory]: timeout: 0.500000s
[ros2_control_node-3] [INFO] [1712164660.626498494] [resource_manager]: Successful initialization of hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.626700162] [resource_manager]: Loading hardware 'FrankaHardwareInterface' 
[ros2_control_node-3] [INFO] [1712164660.630059985] [resource_manager]: Initialize hardware 'FrankaHardwareInterface' 
[ros2_control_node-3] [INFO] [1712164660.630212531] [FrankaHardwareInterface]: Connecting to robot at "192.168.106.99" ...
[ros2_control_node-3] [INFO] [1712164660.641552319] [FrankaHardwareInterface]: Successfully connected to robot
[ros2_control_node-3] [INFO] [1712164660.648836041] [service_server]: Service started
[ros2_control_node-3] [INFO] [1712164660.749267975] [resource_manager]: Successful initialization of hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.750133393] [resource_manager]: 'configure' hardware 'FrankaHardwareInterface' 
[ros2_control_node-3] [INFO] [1712164660.750163984] [resource_manager]: Successful 'configure' of hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.750247339] [resource_manager]: 'activate' hardware 'FrankaHardwareInterface' 
[ros2_control_node-3] [INFO] [1712164660.750535262] [FrankaHardwareInterface]: Started
[ros2_control_node-3] [INFO] [1712164660.750546509] [resource_manager]: Successful 'activate' of hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.750566727] [resource_manager]: 'configure' hardware 'RobotiqGripperHardwareInterface' 
[ros2_control_node-3] [INFO] [1712164660.751150111] [resource_manager]: Successful 'configure' of hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.751175182] [resource_manager]: 'activate' hardware 'RobotiqGripperHardwareInterface' 
[ros2_control_node-3] [INFO] [1712164660.751183960] [DefaultDriver]: Deactivate...
[ros2_control_node-3] [INFO] [1712164660.764802259] [DefaultDriver]: Activate...
[joint_state_publisher-1] [INFO] [1712164660.769479224] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
[ros2_control_node-3] [INFO] [1712164662.813675508] [RobotiqGripperHardwareInterface]: Robotiq Gripper successfully activated!
[ros2_control_node-3] [INFO] [1712164662.813702521] [resource_manager]: Successful 'activate' of hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164662.817418470] [controller_manager]: update rate is 500 Hz
[ros2_control_node-3] [INFO] [1712164662.824936177] [controller_manager]: Loading controller 'panda_state_broadcaster'
[ros2_control_node-3] [INFO] [1712164662.830809499] [controller_manager]: Loading controller 'panda_jtc_controller'
[ros2 run controller_manager spawner panda_state_broadcaster-5] [INFO] [1712164662.831143190] [spawner_panda_state_broadcaster]: Loaded panda_state_broadcaster
[ros2_control_node-3] [WARN] [1712164662.840721283] [panda_jtc_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ros2_control_node-3] [INFO] [1712164662.843069014] [controller_manager]: Configuring controller 'panda_state_broadcaster'
[ros2 run controller_manager spawner panda_jtc_controller-4] [INFO] [1712164662.843093477] [spawner_panda_jtc_controller]: Loaded panda_jtc_controller
[ros2_control_node-3] [INFO] [1712164662.843167933] [panda_state_broadcaster]: Publishing state interfaces defined in 'joints' and 'interfaces' parameters.
[ros2_control_node-3] [WARN] [1712164662.843215249] [panda_state_broadcaster]: Mapping from 'position' to interface 'position' will not be done, because 'position' is defined in 'interface' parameter.
[ros2_control_node-3] [WARN] [1712164662.843222882] [panda_state_broadcaster]: Mapping from 'velocity' to interface 'velocity' will not be done, because 'velocity' is defined in 'interface' parameter.
[ros2_control_node-3] [WARN] [1712164662.843250527] [panda_state_broadcaster]: Mapping from 'effort' to interface 'effort' will not be done, because 'effort' is defined in 'interface' parameter.
[ros2_control_node-3] [INFO] [1712164662.846703706] [controller_manager]: Loading controller 'robotiq_state_broadcaster'
[ros2_control_node-3] [INFO] [1712164662.850643101] [controller_manager]: Configuring controller 'panda_jtc_controller'
[ros2 run controller_manager spawner robotiq_state_broadcaster-6] [INFO] [1712164662.851000237] [spawner_robotiq_state_broadcaster]: Loaded robotiq_state_broadcaster
[ros2_control_node-3] [INFO] [1712164662.851205383] [panda_jtc_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-3] [WARN] [1712164662.851574086] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.851767147] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.851883758] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.851986646] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.852091575] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.852188067] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.852289260] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [INFO] [1712164662.852544336] [panda_jtc_controller]: Command interfaces are [effort] and state interfaces are [position velocity].
[ros2_control_node-3] [INFO] [1712164662.852574192] [panda_jtc_controller]: Using 'splines' interpolation method.
[ros2_control_node-3] [INFO] [1712164662.854284689] [panda_jtc_controller]: Controller state will be published at 50.00 Hz.
[ros2_control_node-3] [INFO] [1712164662.856152021] [panda_jtc_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2_control_node-3] [INFO] [1712164662.860786047] [controller_manager]: Configuring controller 'robotiq_state_broadcaster'
[ros2_control_node-3] [INFO] [1712164662.860843073] [robotiq_state_broadcaster]: Publishing state interfaces defined in 'joints' and 'interfaces' parameters.
[ros2_control_node-3] [WARN] [1712164662.860870905] [robotiq_state_broadcaster]: Mapping from 'position' to interface 'position' will not be done, because 'position' is defined in 'interface' parameter.
[ros2_control_node-3] [WARN] [1712164662.860879527] [robotiq_state_broadcaster]: Mapping from 'velocity' to interface 'velocity' will not be done, because 'velocity' is defined in 'interface' parameter.
[ros2 run controller_manager spawner panda_state_broadcaster-5] [INFO] [1712164662.867305831] [spawner_panda_state_broadcaster]: Configured and activated panda_state_broadcaster
[ros2 run controller_manager spawner panda_jtc_controller-4] [INFO] [1712164662.918119995] [spawner_panda_jtc_controller]: Configured and activated panda_jtc_controller
[ros2_control_node-3] [INFO] [1712164662.919843507] [controller_manager]: Loading controller 'robotiq_gripper_controller'
[ros2 run controller_manager spawner robotiq_state_broadcaster-6] [INFO] [1712164662.920103455] [spawner_robotiq_state_broadcaster]: Configured and activated robotiq_state_broadcaster
[ros2 run controller_manager spawner robotiq_gripper_controller-7] [INFO] [1712164662.924939357] [spawner_robotiq_gripper_controller]: Loaded robotiq_gripper_controller
[ros2_control_node-3] [INFO] [1712164662.925491189] [controller_manager]: Configuring controller 'robotiq_gripper_controller'
[ros2_control_node-3] [INFO] [1712164662.925572228] [robotiq_gripper_controller]: Action status changes will be monitored at 20Hz.
[ros2_control_node-3] [FATAL] [1712164662.927536989] [FrankaHardwareInterface]: Invalid number of position interfaces to start. Expected 7, given 1
[ros2_control_node-3] terminate called after throwing an instance of 'std::invalid_argument'
[ros2_control_node-3]   what():  Invalid number of position interfaces to start. Expected 7, given 1
[ros2_control_node-3] Stack trace (most recent call last) in thread 35982:
[ros2_control_node-3] #31   Object "", at 0xffffffffffffffff, in 
[ros2_control_node-3] #30   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c45a84f, in 
[ros2_control_node-3] #29   Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c3c8ac2, in 
[ros2_control_node-3] #28   Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c65b252, in 
[ros2_control_node-3] #27   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fc02c89a279, in rclcpp::executors::MultiThreadedExecutor::run(unsigned long)
[ros2_control_node-3] #26   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fc02c892ff5, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[ros2_control_node-3] #25   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fc02c892c89, in rclcpp::Executor::execute_service(std::shared_ptr<rclcpp::ServiceBase>)
[ros2_control_node-3] #24   Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fc02c8952a5, in 
[ros2_control_node-3] #23   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d176972, in rclcpp::Service<controller_manager_msgs::srv::SwitchController>::handle_request(std::shared_ptr<rmw_request_id_s>, std::shared_ptr<void>)
[ros2_control_node-3] #22   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d17e413, in rclcpp::AnyServiceCallback<controller_manager_msgs::srv::SwitchController>::dispatch(std::shared_ptr<rclcpp::Service<controller_manager_msgs::srv::SwitchController> > const&, std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >)
[ros2_control_node-3] #21   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d188ae4, in std::function<void (std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)>::operator()(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >) const
[ros2_control_node-3] #20   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d0e1ea8, in std::_Function_handler<void (std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >), std::_Bind<void (controller_manager::ControllerManager::*(controller_manager::ControllerManager*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)> >::_M_invoke(std::_Any_data const&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >&&)
[ros2_control_node-3] #19   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d0f2ded, in std::enable_if<is_invocable_r_v<void, std::_Bind<void (controller_manager::ControllerManager::*(controller_manager::ControllerManager*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > > >, void>::type std::__invoke_r<void, std::_Bind<void (controller_manager::ControllerManager::*(controller_manager::ControllerManager*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > > >(std::_Bind<void (controller_manager::ControllerManager::*(controller_manager::ControllerManager*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >&&)
[ros2_control_node-3] #18   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d10263c, in void std::__invoke_impl<void, std::_Bind<void (controller_manager::ControllerManager::*(controller_manager::ControllerManager*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > > >(std::__invoke_other, std::_Bind<void (controller_manager::ControllerManager::*(controller_manager::ControllerManager*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >&&)
[ros2_control_node-3] #17   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d10dbb8, in void std::_Bind<void (controller_manager::ControllerManager::*(controller_manager::ControllerManager*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)>::operator()<std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >, void>(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >&&)
[ros2_control_node-3] #16   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d118730, in void std::_Bind<void (controller_manager::ControllerManager::*(controller_manager::ControllerManager*, std::_Placeholder<1>, std::_Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)>::__call<void, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >&&, 0ul, 1ul, 2ul>(std::tuple<std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >&&>&&, std::_Index_tuple<0ul, 1ul, 2ul>)
[ros2_control_node-3] #15   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d123246, in std::__invoke_result<void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > > >::type std::__invoke<void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > > >(void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >&&)
[ros2_control_node-3] #14   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d130fd6, in void std::__invoke_impl<void, void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > > >(std::__invoke_memfun_deref, void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >&&)
[ros2_control_node-3] #13   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d047c1a, in controller_manager::ControllerManager::switch_controller_service_cb(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator<void> > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator<void> > >)
[ros2_control_node-3] #12   Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d03ef93, in controller_manager::ControllerManager::switch_controller(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, int, bool, rclcpp::Duration const&)
[ros2_control_node-3] #11   Object "/root/panda_ws/install/hardware_interface/lib/libhardware_interface.so", at 0x7fc02c1ed7fd, in hardware_interface::ResourceManager::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)
[ros2_control_node-3] #10   Object "/root/panda_ws/install/hardware_interface/lib/libhardware_interface.so", at 0x7fc02c1ecf69, in auto hardware_interface::ResourceManager::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)::{lambda(auto:1&)#4}::operator()<std::vector<hardware_interface::System, std::allocator<hardware_interface::System> > >(std::vector<hardware_interface::System, std::allocator<hardware_interface::System> >&) const
[ros2_control_node-3] #9    Object "/root/panda_ws/install/hardware_interface/lib/libhardware_interface.so", at 0x7fc02c2338e8, in hardware_interface::System::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)
[ros2_control_node-3] #8    Object "/root/panda_ws/install/franka_hardware/lib/libfranka_hardware.so", at 0x7fc019f28da5, in franka_hardware::FrankaHardwareInterface::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)
[ros2_control_node-3] #7    Object "/root/panda_ws/install/franka_hardware/lib/libfranka_hardware.so", at 0x7fc019f28a99, in franka_hardware::FrankaHardwareInterface::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> >, std::allocator<std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > > > const&)::{lambda(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned long, unsigned long)#2}::operator()(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, unsigned long, unsigned long) const
[ros2_control_node-3] #6    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c62d4d7, in __cxa_throw
[ros2_control_node-3] #5    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c62d276, in std::terminate()
[ros2_control_node-3] #4    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c62d20b, in 
[ros2_control_node-3] #3    Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c621b9d, in 
[ros2_control_node-3] #2    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c35c7f2, in abort
[ros2_control_node-3] #1    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c376475, in raise
[ros2_control_node-3] #0    Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c3ca9fc, in pthread_kill
[ros2_control_node-3] Aborted (Signal sent by tkill() 35804 0)
[ERROR] [ros2_control_node-3]: process has died [pid 35804, exit code -6, cmd '/root/panda_ws/install/controller_manager/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_oad8yatj --params-file /root/panda_ws/install/franka_robotiq_moveit_config/share/franka_robotiq_moveit_config/config/ros2_controllers.yaml'].
[INFO] [ros2 run controller_manager spawner panda_state_broadcaster-5]: process has finished cleanly [pid 35808]
[INFO] [ros2 run controller_manager spawner robotiq_state_broadcaster-6]: process has finished cleanly [pid 35810]
[INFO] [ros2 run controller_manager spawner panda_jtc_controller-4]: process has finished cleanly [pid 35806]

root@franka-control:/panda_ws# ros2 launch panda_control_demos control_server_debug.launch.py robot_ip:=192.168.106.99 use_fake_hardware:=false
[INFO] [launch]: All log files can be found below /root/.ros/log/2024-04-03-17-17-40-409065-franka-control-35799
[INFO] [launch]: Default logging verbosity is set to INFO
[INFO] [joint_state_publisher-1]: process started with pid [35800]
[INFO] [robot_state_publisher-2]: process started with pid [35802]
[INFO] [ros2_control_node-3]: process started with pid [35804]
[INFO] [ros2 run controller_manager spawner panda_jtc_controller-4]: process started with pid [35806]
[INFO] [ros2 run controller_manager spawner panda_state_broadcaster-5]: process started with pid [35808]
[INFO] [ros2 run controller_manager spawner robotiq_state_broadcaster-6]: process started with pid [35810]
[INFO] [ros2 run controller_manager spawner robotiq_gripper_controller-7]: process started with pid [35812]
[robot_state_publisher-2] [INFO] [1712164660.608402668] [panda_state_publisher]: got segment panda_link0
[robot_state_publisher-2] [INFO] [1712164660.608460540] [panda_state_publisher]: got segment panda_link1
[robot_state_publisher-2] [INFO] [1712164660.608466522] [panda_state_publisher]: got segment panda_link2
[robot_state_publisher-2] [INFO] [1712164660.608469562] [panda_state_publisher]: got segment panda_link3
[robot_state_publisher-2] [INFO] [1712164660.608472392] [panda_state_publisher]: got segment panda_link4
[robot_state_publisher-2] [INFO] [1712164660.608475100] [panda_state_publisher]: got segment panda_link5
[robot_state_publisher-2] [INFO] [1712164660.608477667] [panda_state_publisher]: got segment panda_link6
[robot_state_publisher-2] [INFO] [1712164660.608480275] [panda_state_publisher]: got segment panda_link7
[robot_state_publisher-2] [INFO] [1712164660.608483043] [panda_state_publisher]: got segment panda_link8
[robot_state_publisher-2] [INFO] [1712164660.608485668] [panda_state_publisher]: got segment robotiq_85_base_link
[robot_state_publisher-2] [INFO] [1712164660.608488616] [panda_state_publisher]: got segment robotiq_85_left_finger_link
[robot_state_publisher-2] [INFO] [1712164660.608491366] [panda_state_publisher]: got segment robotiq_85_left_finger_tip_link
[robot_state_publisher-2] [INFO] [1712164660.608494288] [panda_state_publisher]: got segment robotiq_85_left_inner_knuckle_link
[robot_state_publisher-2] [INFO] [1712164660.608497094] [panda_state_publisher]: got segment robotiq_85_left_knuckle_link
[robot_state_publisher-2] [INFO] [1712164660.608499798] [panda_state_publisher]: got segment robotiq_85_right_finger_link
[robot_state_publisher-2] [INFO] [1712164660.608503095] [panda_state_publisher]: got segment robotiq_85_right_finger_tip_link
[robot_state_publisher-2] [INFO] [1712164660.608507831] [panda_state_publisher]: got segment robotiq_85_right_inner_knuckle_link
[robot_state_publisher-2] [INFO] [1712164660.608513546] [panda_state_publisher]: got segment robotiq_85_right_knuckle_link
[ros2_control_node-3] [WARN] [1712164660.624893052] [controller_manager]: [Deprecated] Passing the robot description parameter directly to the control_manager node is deprecated. Use '/robot_description' topic from 'robot_state_publisher' instead.
[ros2_control_node-3] [INFO] [1712164660.625457791] [resource_manager]: Loading hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.626245168] [resource_manager]: Initialize hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.626360955] [DefaultDriverFactory]: Reading slave_address...
[ros2_control_node-3] [INFO] [1712164660.626367766] [DefaultDriverFactory]: slave_address: 9
[ros2_control_node-3] [INFO] [1712164660.626369915] [DefaultDriverFactory]: Reading gripper_speed_multiplier...
[ros2_control_node-3] [INFO] [1712164660.626375080] [DefaultDriverFactory]: gripper_speed_multiplier: 1.000000s
[ros2_control_node-3] [INFO] [1712164660.626379117] [DefaultDriverFactory]: Reading gripper_force_multiplier...
[ros2_control_node-3] [INFO] [1712164660.626381252] [DefaultDriverFactory]: gripper_force_multiplier: 0.500000s
[ros2_control_node-3] [INFO] [1712164660.626385329] [DefaultSerialFactory]: Reading COM_port...
[ros2_control_node-3] [INFO] [1712164660.626388065] [DefaultSerialFactory]: COM_port: /dev/ttyUSB0
[ros2_control_node-3] [INFO] [1712164660.626389579] [DefaultSerialFactory]: Reading baudrate...
[ros2_control_node-3] [INFO] [1712164660.626391030] [DefaultSerialFactory]: baudrate: 115200bps
[ros2_control_node-3] [INFO] [1712164660.626392457] [DefaultSerialFactory]: Reading timeout...
[ros2_control_node-3] [INFO] [1712164660.626393926] [DefaultSerialFactory]: timeout: 0.500000s
[ros2_control_node-3] [INFO] [1712164660.626498494] [resource_manager]: Successful initialization of hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.626700162] [resource_manager]: Loading hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.630059985] [resource_manager]: Initialize hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.630212531] [FrankaHardwareInterface]: Connecting to robot at "192.168.106.99" ...
[ros2_control_node-3] [INFO] [1712164660.641552319] [FrankaHardwareInterface]: Successfully connected to robot
[ros2_control_node-3] [INFO] [1712164660.648836041] [service_server]: Service started
[ros2_control_node-3] [INFO] [1712164660.749267975] [resource_manager]: Successful initialization of hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.750133393] [resource_manager]: 'configure' hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.750163984] [resource_manager]: Successful 'configure' of hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.750247339] [resource_manager]: 'activate' hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.750535262] [FrankaHardwareInterface]: Started
[ros2_control_node-3] [INFO] [1712164660.750546509] [resource_manager]: Successful 'activate' of hardware 'FrankaHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.750566727] [resource_manager]: 'configure' hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.751150111] [resource_manager]: Successful 'configure' of hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.751175182] [resource_manager]: 'activate' hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164660.751183960] [DefaultDriver]: Deactivate...
[ros2_control_node-3] [INFO] [1712164660.764802259] [DefaultDriver]: Activate...
[joint_state_publisher-1] [INFO] [1712164660.769479224] [joint_state_publisher]: Waiting for robot_description to be published on the robot_description topic...
[ros2_control_node-3] [INFO] [1712164662.813675508] [RobotiqGripperHardwareInterface]: Robotiq Gripper successfully activated!
[ros2_control_node-3] [INFO] [1712164662.813702521] [resource_manager]: Successful 'activate' of hardware 'RobotiqGripperHardwareInterface'
[ros2_control_node-3] [INFO] [1712164662.817418470] [controller_manager]: update rate is 500 Hz
[ros2_control_node-3] [INFO] [1712164662.824936177] [controller_manager]: Loading controller 'panda_state_broadcaster'
[ros2_control_node-3] [INFO] [1712164662.830809499] [controller_manager]: Loading controller 'panda_jtc_controller'
[ros2 run controller_manager spawner panda_state_broadcaster-5] [INFO] [1712164662.831143190] [spawner_panda_state_broadcaster]: Loaded panda_state_broadcaster
[ros2_control_node-3] [WARN] [1712164662.840721283] [panda_jtc_controller]: [Deprecated]: "allow_nonzero_velocity_at_trajectory_end" is set to true. The default behavior will change to false.
[ros2_control_node-3] [INFO] [1712164662.843069014] [controller_manager]: Configuring controller 'panda_state_broadcaster'
[ros2 run controller_manager spawner panda_jtc_controller-4] [INFO] [1712164662.843093477] [spawner_panda_jtc_controller]: Loaded panda_jtc_controller
[ros2_control_node-3] [INFO] [1712164662.843167933] [panda_state_broadcaster]: Publishing state interfaces defined in 'joints' and 'interfaces' parameters.
[ros2_control_node-3] [WARN] [1712164662.843215249] [panda_state_broadcaster]: Mapping from 'position' to interface 'position' will not be done, because 'position' is defined in 'interface' parameter.
[ros2_control_node-3] [WARN] [1712164662.843222882] [panda_state_broadcaster]: Mapping from 'velocity' to interface 'velocity' will not be done, because 'velocity' is defined in 'interface' parameter.
[ros2_control_node-3] [WARN] [1712164662.843250527] [panda_state_broadcaster]: Mapping from 'effort' to interface 'effort' will not be done, because 'effort' is defined in 'interface' parameter.
[ros2_control_node-3] [INFO] [1712164662.846703706] [controller_manager]: Loading controller 'robotiq_state_broadcaster'
[ros2_control_node-3] [INFO] [1712164662.850643101] [controller_manager]: Configuring controller 'panda_jtc_controller'
[ros2 run controller_manager spawner robotiq_state_broadcaster-6] [INFO] [1712164662.851000237] [spawner_robotiq_state_broadcaster]: Loaded robotiq_state_broadcaster
[ros2_control_node-3] [INFO] [1712164662.851205383] [panda_jtc_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
[ros2_control_node-3] [WARN] [1712164662.851574086] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.851767147] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.851883758] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.851986646] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.852091575] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.852188067] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [WARN] [1712164662.852289260] [panda_jtc_controller]: 'ff_velocity_scale' parameters is not defined under 'gains.<joint_name>.' structure. Maybe you are using deprecated format 'ff_velocity_scale/<joint_name>'!
[ros2_control_node-3] [INFO] [1712164662.852544336] [panda_jtc_controller]: Command interfaces are [effort] and state interfaces are [position velocity].
[ros2_control_node-3] [INFO] [1712164662.852574192] [panda_jtc_controller]: Using 'splines' interpolation method.
[ros2_control_node-3] [INFO] [1712164662.854284689] [panda_jtc_controller]: Controller state will be published at 50.00 Hz.
[ros2_control_node-3] [INFO] [1712164662.856152021] [panda_jtc_controller]: Action status changes will be monitored at 20.00 Hz.
[ros2_control_node-3] [INFO] [1712164662.860786047] [controller_manager]: Configuring controller 'robotiq_state_broadcaster'
[ros2_control_node-3] [INFO] [1712164662.860843073] [robotiq_state_broadcaster]: Publishing state interfaces defined in 'joints' and 'interfaces' parameters.
[ros2_control_node-3] [WARN] [1712164662.860870905] [robotiq_state_broadcaster]: Mapping from 'position' to interface 'position' will not be done, because 'position' is defined in 'interface' parameter.
[ros2_control_node-3] [WARN] [1712164662.860879527] [robotiq_state_broadcaster]: Mapping from 'velocity' to interface 'velocity' will not be done, because 'velocity' is defined in 'interface' parameter.
[ros2 run controller_manager spawner panda_state_broadcaster-5] [INFO] [1712164662.867305831] [spawner_panda_state_broadcaster]: Configured and activated panda_state_broadcaster
[ros2 run controller_manager spawner panda_jtc_controller-4] [INFO] [1712164662.918119995] [spawner_panda_jtc_controller]: Configured and activated panda_jtc_controller
[ros2_control_node-3] [INFO] [1712164662.919843507] [controller_manager]: Loading controller 'robotiq_gripper_controller'
[ros2 run controller_manager spawner robotiq_state_broadcaster-6] [INFO] [1712164662.920103455] [spawner_robotiq_state_broadcaster]: Configured and activated robotiq_state_broadcaster
[ros2 run controller_manager spawner robotiq_gripper_controller-7] [INFO] [1712164662.924939357] [spawner_robotiq_gripper_controller]: Loaded robotiq_gripper_controller
[ros2_control_node-3] [INFO] [1712164662.925491189] [controller_manager]: Configuring controller 'robotiq_gripper_controller'
[ros2_control_node-3] [INFO] [1712164662.925572228] [robotiq_gripper_controller]: Action status changes will be monitored at 20Hz.
[ros2_control_node-3] [FATAL] [1712164662.927536989] [FrankaHardwareInterface]: Invalid number of position interfaces to start. Expected 7, given 1
[ros2_control_node-3] terminate called after throwing an instance of 'std::invalid_argument'
[ros2_control_node-3] what(): Invalid number of position interfaces to start. Expected 7, given 1
[ros2_control_node-3] Stack trace (most recent call last) in thread 35982:
[ros2_control_node-3] #31 Object "", at 0xffffffffffffffff, in
[ros2_control_node-3] #30 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c45a84f, in
[ros2_control_node-3] #29 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c3c8ac2, in
[ros2_control_node-3] #28 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c65b252, in
[ros2_control_node-3] #27 Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fc02c89a279, in rclcpp::executors::MultiThreadedExecutor::run(unsigned long)
[ros2_control_node-3] #26 Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fc02c892ff5, in rclcpp::Executor::execute_any_executable(rclcpp::AnyExecutable&)
[ros2_control_node-3] #25 Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fc02c892c89, in rclcpp::Executor::execute_service(std::shared_ptrrclcpp::ServiceBase)
[ros2_control_node-3] #24 Object "/opt/ros/humble/lib/librclcpp.so", at 0x7fc02c8952a5, in
[ros2_control_node-3] #23 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d176972, in rclcpp::Service<controller_manager_msgs::srv::SwitchController>::handle_request(std::shared_ptr<rmw_request_id_s>, std::shared_ptr)
[ros2_control_node-3] #22 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d17e413, in rclcpp::AnyServiceCallback<controller_manager_msgs::srv::SwitchController>::dispatch(std::shared_ptr<rclcpp::Service<controller_manager_msgs::srv::SwitchController> > const&, std::shared_ptr<rmw_request_id_s> const&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >)
[ros2_control_node-3] #21 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d188ae4, in std::function<void (std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >)>::operator()(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >) const
[ros2_control_node-3] #20 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d0e1ea8, in std::Function_handler<void (std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >), std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >)> >::M_invoke(std::Any_data const&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >&&)
[ros2_control_node-3] #19 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d0f2ded, in std::enable_if<is_invocable_r_v<void, std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > > >, void>::type std::invoke_r<void, std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > > >(std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >&&)
[ros2_control_node-3] #18 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d10263c, in void std::invoke_impl<void, std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > > >(std::invoke_other, std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >)>&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >&&)
[ros2_control_node-3] #17 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d10dbb8, in void std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >)>::operator()<std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >, void>(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >&&)
[ros2_control_node-3] #16 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d118730, in void std::Bind<void (controller_manager::ControllerManager::(controller_manager::ControllerManager, std::Placeholder<1>, std::Placeholder<2>))(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >)>::call<void, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >&&, 0ul, 1ul, 2ul>(std::tuple<std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >&&>&&, std::Index_tuple<0ul, 1ul, 2ul>)
[ros2_control_node-3] #15 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d123246, in std::invoke_result<void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > > >::type std::invoke<void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > > >(void (controller_manager::ControllerManager::&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >), controller_manager::ControllerManager&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >&&)
[ros2_control_node-3] #14 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d130fd6, in void std::invoke_impl<void, void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > > >(std::invoke_memfun_deref, void (controller_manager::ControllerManager::*&)(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response<std::allocator > >), controller_manager::ControllerManager*&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >&&, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >&&)
[ros2_control_node-3] #13 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d047c1a, in controller_manager::ControllerManager::switch_controller_service_cb(std::shared_ptr<controller_manager_msgs::srv::SwitchController_Request_<std::allocator > >, std::shared_ptr<controller_manager_msgs::srv::SwitchController_Response_<std::allocator > >)
[ros2_control_node-3] #12 Object "/root/panda_ws/install/controller_manager/lib/libcontroller_manager.so", at 0x7fc02d03ef93, in controller_manager::ControllerManager::switch_controller(std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&, int, bool, rclcpp::Duration const&)
[ros2_control_node-3] #11 Object "/root/panda_ws/install/hardware_interface/lib/libhardware_interface.so", at 0x7fc02c1ed7fd, in hardware_interface::ResourceManager::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&)
[ros2_control_node-3] #10 Object "/root/panda_ws/install/hardware_interface/lib/libhardware_interface.so", at 0x7fc02c1ecf69, in auto hardware_interface::ResourceManager::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&)::{lambda(auto:1&)#4}::operator()<std::vector<hardware_interface::System, std::allocator<hardware_interface::System> > >(std::vector<hardware_interface::System, std::allocator<hardware_interface::System> >&) const
[ros2_control_node-3] #9 Object "/root/panda_ws/install/hardware_interface/lib/libhardware_interface.so", at 0x7fc02c2338e8, in hardware_interface::System::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&)
[ros2_control_node-3] #8 Object "/root/panda_ws/install/franka_hardware/lib/libfranka_hardware.so", at 0x7fc019f28da5, in franka_hardware::FrankaHardwareInterface::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&)
[ros2_control_node-3] #7 Object "/root/panda_ws/install/franka_hardware/lib/libfranka_hardware.so", at 0x7fc019f28a99, in franka_hardware::FrankaHardwareInterface::prepare_command_mode_switch(std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&, std::vector<std::__cxx11::basic_string<char, std::char_traits, std::allocator >, std::allocator<std::__cxx11::basic_string<char, std::char_traits, std::allocator > > > const&)::{lambda(std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, unsigned long, unsigned long)#2}::operator()(std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, std::__cxx11::basic_string<char, std::char_traits, std::allocator > const&, unsigned long, unsigned long) const
[ros2_control_node-3] #6 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c62d4d7, in __cxa_throw
[ros2_control_node-3] #5 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c62d276, in std::terminate()
[ros2_control_node-3] #4 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c62d20b, in
[ros2_control_node-3] #3 Object "/usr/lib/x86_64-linux-gnu/libstdc++.so.6.0.30", at 0x7fc02c621b9d, in
[ros2_control_node-3] #2 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c35c7f2, in abort
[ros2_control_node-3] #1 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c376475, in raise
[ros2_control_node-3] #0 Object "/usr/lib/x86_64-linux-gnu/libc.so.6", at 0x7fc02c3ca9fc, in pthread_kill
[ros2_control_node-3] Aborted (Signal sent by tkill() 35804 0)
[ERROR] [ros2_control_node-3]: process has died [pid 35804, exit code -6, cmd '/root/panda_ws/install/controller_manager/lib/controller_manager/ros2_control_node --ros-args --params-file /tmp/launch_params_oad8yatj --params-file /root/panda_ws/install/franka_robotiq_moveit_config/share/franka_robotiq_moveit_config/config/ros2_controllers.yaml'].
[INFO] [ros2 run controller_manager spawner panda_state_broadcaster-5]: process has finished cleanly [pid 35808]
[INFO] [ros2 run controller_manager spawner robotiq_state_broadcaster-6]: process has finished cleanly [pid 35810]
[INFO] [ros2 run controller_manager spawner panda_jtc_controller-4]: process has finished cleanly [pid 35806]

[peterdavidfagan]

Example launch file combing panda + robotiq gripper.

import os
from launch import LaunchDescription
from launch.actions import OpaqueFunction, IncludeLaunchDescription, DeclareLaunchArgument
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.launch_description_sources import load_python_launch_file_as_module
from launch.substitutions import (
    Command,
    FindExecutable,
    LaunchConfiguration,
    PathJoinSubstitution,
)
import launch_ros
from launch_ros.substitutions import FindPackageShare
from launch_ros.actions import Node, SetParameter
from launch.actions import ExecuteProcess
from ament_index_python.packages import get_package_share_directory
from moveit_configs_utils import MoveItConfigsBuilder


def generate_launch_description():
    
    # declare parameter for using robot ip
    robot_ip = DeclareLaunchArgument(
        "robot_ip",
        default_value="192.168.106.99",
        description="Robot IP",
    )

    # declare parameter for using gripper
    use_gripper = DeclareLaunchArgument(
        "use_gripper",
        default_value="false",
        description="Use gripper",
    )

    # declare parameter for using fake controller
    use_fake_hardware = DeclareLaunchArgument(
        "use_fake_hardware",
        default_value="true", # default to fake hardware (Important: that user is explicit with intention of launching real hardware!)
        description="Use fake hardware",
    )


    # panda arm config
    moveit_config = (
            MoveItConfigsBuilder(robot_name="panda", package_name="franka_robotiq_moveit_config")
            .robot_description(file_path=get_package_share_directory("franka_robotiq_description") + "/urdf/robot.urdf.xacro",
                mappings={
                    "robot_ip": LaunchConfiguration("robot_ip"),
                    "robotiq_gripper": LaunchConfiguration("use_gripper"),
                    "use_fake_hardware": LaunchConfiguration("use_fake_hardware"),
                    })
            .robot_description_semantic("config/panda.srdf.xacro")
            .trajectory_execution("config/moveit_controllers.yaml")
            .to_moveit_configs()
            )

    panda_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="panda_state_publisher",
        output="both",
        parameters=[moveit_config.robot_description],
        namespace="panda",
    )

    panda_controller_config = os.path.join(
        get_package_share_directory("franka_robotiq_moveit_config"),
        "config",
        "ros2_controllers.yaml",
    )

    panda_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[moveit_config.robot_description, panda_controller_config],
        output="both",
        namespace="panda"
    )


    # gripper config
    robotiq_xacro = os.path.join(
            get_package_share_directory("robotiq_description"),
            "urdf",
            "robotiq_2f_85_gripper.urdf.xacro",
            )

    robot_description_content = Command(
        [
            PathJoinSubstitution([FindExecutable(name="xacro")]),
            " ",
            robotiq_xacro,
            " ",
            "use_fake_hardware:=false",
        ]
    )

    robotiq_description_param = {
        "robot_description": launch_ros.parameter_descriptions.ParameterValue(
            robot_description_content, value_type=str
        )
    }

    robotiq_state_publisher = Node(
        package="robot_state_publisher",
        executable="robot_state_publisher",
        name="robotiq_state_publisher",
        output="both",
        parameters=[robotiq_description_param],
        namespace="robotiq",
    )

    robotiq_controller_config = os.path.join(
        get_package_share_directory("franka_robotiq_moveit_config"),
        "config",
        "robotiq_controllers.yaml",
    )

    robotiq_control_node = Node(
        package="controller_manager",
        executable="ros2_control_node",
        parameters=[robotiq_description_param, robotiq_controller_config],
        output="both",
        namespace="robotiq",
    )

    # overall joint state
    joint_state_publisher = Node(
        package='joint_state_publisher',
        executable='joint_state_publisher',
        name='joint_state_publisher',
        output='screen',
        parameters=[
                {'source_list': ['panda_state_broadcaster/joint_states', 'robotiq_state_broadcaster/joint_states'],
                 'rate': 30}],
    )


    load_panda_controllers = []
    for controller in [
        'panda_jtc_controller',
        'panda_state_broadcaster',
    ]:
        load_panda_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {} -c /panda/controller_manager".format(controller)],
                shell=True,
                output="screen",
            )
        ]
 load_robotiq_controllers = []
    for controller in [
        'robotiq_state_broadcaster',
        'robotiq_gripper_controller',
        'robotiq_activation_controller',
    ]:
        load_robotiq_controllers += [
            ExecuteProcess(
                cmd=["ros2 run controller_manager spawner {} -c /robotiq/controller_manager".format(controller)],
                shell=True,
                output="screen",
            )
        ]

    return LaunchDescription(
        [
            robot_ip, 
            use_gripper,
            use_fake_hardware,
            joint_state_publisher,
            panda_state_publisher,
            panda_control_node,
            robotiq_state_publisher,
            robotiq_control_node,
        ]
        + load_panda_controllers
        + load_robotiq_controllers
        )