From 28522d2ecc761b8c0f3239e28cbb3031ba803f01 Mon Sep 17 00:00:00 2001 From: Giang Nguyen Date: Fri, 9 Aug 2024 17:52:55 +0200 Subject: [PATCH] Fix bug for ros2_control --- .../scripts/launch_ros.py | 8 +- multiverse/resources/muv/pr2_ros2_control.muv | 88 +++++++++---------- .../multiverse_control/CMakeLists.txt | 2 +- 3 files changed, 49 insertions(+), 49 deletions(-) diff --git a/multiverse/modules/multiverse_connectors/scripts/launch_ros.py b/multiverse/modules/multiverse_connectors/scripts/launch_ros.py index 9b336de5..bf581df0 100644 --- a/multiverse/modules/multiverse_connectors/scripts/launch_ros.py +++ b/multiverse/modules/multiverse_connectors/scripts/launch_ros.py @@ -260,6 +260,8 @@ def _run_controller_node(self, ros_control: Dict[str, Any]) -> subprocess.Popen: continue joint_element = ET.Element("joint") + ros2_control_element.append(joint_element) + joint_element.set("name", joint.name) state_interface = ET.Element("state_interface") @@ -269,7 +271,7 @@ def _run_controller_node(self, ros_control: Dict[str, Any]) -> subprocess.Popen: if joint.name in joint_state: initial_value_param.text = str(joint_state[joint.name]) else: - initial_value_param.text = 0.0 + initial_value_param.text = "0.0" state_interface.append(initial_value_param) joint_element.append(state_interface) @@ -309,7 +311,7 @@ def _run_controller_node(self, ros_control: Dict[str, Any]) -> subprocess.Popen: if joint.name in joint_state: initial_value_param.text = str(joint_state[joint.name]) else: - initial_value_param.text = 0.0 + initial_value_param.text = "0.0" state_interface.append(initial_value_param) joint_element.append(state_interface) @@ -341,8 +343,6 @@ def _run_controller_node(self, ros_control: Dict[str, Any]) -> subprocess.Popen: state_interface.append(max_effort_param) joint_element.append(state_interface) - ros2_control_element.append(joint_element) - robot_element.append(ros2_control_element) tmp_urdf_path = os.path.join("/tmp", os.path.basename(robot_urdf_path)) diff --git a/multiverse/resources/muv/pr2_ros2_control.muv b/multiverse/resources/muv/pr2_ros2_control.muv index 0f8317ef..45afba66 100644 --- a/multiverse/resources/muv/pr2_ros2_control.muv +++ b/multiverse/resources/muv/pr2_ros2_control.muv @@ -15,8 +15,8 @@ simulations: path: floor.xml robots: pr2: - path: pr2/pr2_with_pos_control.xml - # path: pr2/pr2_with_vel_control.xml + path: pr2/pr2_with_vel_control.xml + # path: pr2/pr2_with_pos_control.xml apply: body: pr2: @@ -38,41 +38,41 @@ multiverse_clients: body: ["position", "quaternion"] joint: ["joint_rvalue", "joint_tvalue", "joint_angular_velocity", "joint_linear_velocity", "joint_force", "joint_torque"] receive: - r_shoulder_pan_actuator: ["cmd_joint_rvalue"] - r_shoulder_lift_actuator: ["cmd_joint_rvalue"] - r_upper_arm_roll_actuator: ["cmd_joint_rvalue"] - r_elbow_flex_actuator: ["cmd_joint_rvalue"] - r_forearm_roll_actuator: ["cmd_joint_rvalue"] - r_wrist_flex_actuator: ["cmd_joint_rvalue"] - r_wrist_roll_actuator: ["cmd_joint_rvalue"] - l_shoulder_pan_actuator: ["cmd_joint_rvalue"] - l_shoulder_lift_actuator: ["cmd_joint_rvalue"] - l_upper_arm_roll_actuator: ["cmd_joint_rvalue"] - l_elbow_flex_actuator: ["cmd_joint_rvalue"] - l_forearm_roll_actuator: ["cmd_joint_rvalue"] - l_wrist_flex_actuator: ["cmd_joint_rvalue"] - l_wrist_roll_actuator: ["cmd_joint_rvalue"] - torso_lift_actuator: ["cmd_joint_tvalue"] - head_pan_actuator: ["cmd_joint_rvalue"] - head_tilt_actuator: ["cmd_joint_rvalue"] + r_shoulder_pan_actuator: ["cmd_joint_angular_velocity"] + r_shoulder_lift_actuator: ["cmd_joint_angular_velocity"] + r_upper_arm_roll_actuator: ["cmd_joint_angular_velocity"] + r_elbow_flex_actuator: ["cmd_joint_angular_velocity"] + r_forearm_roll_actuator: ["cmd_joint_angular_velocity"] + r_wrist_flex_actuator: ["cmd_joint_angular_velocity"] + r_wrist_roll_actuator: ["cmd_joint_angular_velocity"] + l_shoulder_pan_actuator: ["cmd_joint_angular_velocity"] + l_shoulder_lift_actuator: ["cmd_joint_angular_velocity"] + l_upper_arm_roll_actuator: ["cmd_joint_angular_velocity"] + l_elbow_flex_actuator: ["cmd_joint_angular_velocity"] + l_forearm_roll_actuator: ["cmd_joint_angular_velocity"] + l_wrist_flex_actuator: ["cmd_joint_angular_velocity"] + l_wrist_roll_actuator: ["cmd_joint_angular_velocity"] + torso_lift_actuator: ["cmd_joint_linear_velocity"] + head_pan_actuator: ["cmd_joint_angular_velocity"] + head_tilt_actuator: ["cmd_joint_angular_velocity"] - # r_shoulder_pan_actuator: ["cmd_joint_angular_velocity"] - # r_shoulder_lift_actuator: ["cmd_joint_angular_velocity"] - # r_upper_arm_roll_actuator: ["cmd_joint_angular_velocity"] - # r_elbow_flex_actuator: ["cmd_joint_angular_velocity"] - # r_forearm_roll_actuator: ["cmd_joint_angular_velocity"] - # r_wrist_flex_actuator: ["cmd_joint_angular_velocity"] - # r_wrist_roll_actuator: ["cmd_joint_angular_velocity"] - # l_shoulder_pan_actuator: ["cmd_joint_angular_velocity"] - # l_shoulder_lift_actuator: ["cmd_joint_angular_velocity"] - # l_upper_arm_roll_actuator: ["cmd_joint_angular_velocity"] - # l_elbow_flex_actuator: ["cmd_joint_angular_velocity"] - # l_forearm_roll_actuator: ["cmd_joint_angular_velocity"] - # l_wrist_flex_actuator: ["cmd_joint_angular_velocity"] - # l_wrist_roll_actuator: ["cmd_joint_angular_velocity"] - # torso_lift_actuator: ["cmd_joint_linear_velocity"] - # head_pan_actuator: ["cmd_joint_angular_velocity"] - # head_tilt_actuator: ["cmd_joint_angular_velocity"] + # r_shoulder_pan_actuator: ["cmd_joint_rvalue"] + # r_shoulder_lift_actuator: ["cmd_joint_rvalue"] + # r_upper_arm_roll_actuator: ["cmd_joint_rvalue"] + # r_elbow_flex_actuator: ["cmd_joint_rvalue"] + # r_forearm_roll_actuator: ["cmd_joint_rvalue"] + # r_wrist_flex_actuator: ["cmd_joint_rvalue"] + # r_wrist_roll_actuator: ["cmd_joint_rvalue"] + # l_shoulder_pan_actuator: ["cmd_joint_rvalue"] + # l_shoulder_lift_actuator: ["cmd_joint_rvalue"] + # l_upper_arm_roll_actuator: ["cmd_joint_rvalue"] + # l_elbow_flex_actuator: ["cmd_joint_rvalue"] + # l_forearm_roll_actuator: ["cmd_joint_rvalue"] + # l_wrist_flex_actuator: ["cmd_joint_rvalue"] + # l_wrist_roll_actuator: ["cmd_joint_rvalue"] + # torso_lift_actuator: ["cmd_joint_tvalue"] + # head_pan_actuator: ["cmd_joint_rvalue"] + # head_tilt_actuator: ["cmd_joint_rvalue"] ros2: ros2_control: @@ -110,14 +110,14 @@ multiverse_clients: controllers: spawner: - joint_state_broadcaster - right_arm_trajectory_controller - left_arm_trajectory_controller - torso_trajectory_controller - head_trajectory_controller - # right_arm_velocity_controller - # left_arm_velocity_controller - # torso_velocity_controller - # head_velocity_controller + right_arm_velocity_controller + left_arm_velocity_controller + torso_velocity_controller + head_velocity_controller + # right_arm_trajectory_controller + # left_arm_trajectory_controller + # torso_trajectory_controller + # head_trajectory_controller ros2_run: rviz2: diff --git a/multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt b/multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt index ca5f6399..b0e53b60 100644 --- a/multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt +++ b/multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt @@ -87,7 +87,7 @@ install(TARGETS multiverse_control ) execute_process( - COMMAND cp ${MULTIVERSE_DIR}/lib/libstdc++/libmultiverse_client_json.so ${MULTIVERSE_DIR}/../multiverse_ws2/install/multiverse_control/lib/libmultiverse_client_json.so + COMMAND cp -f ${MULTIVERSE_DIR}/lib/libstdc++/libmultiverse_client_json.so ${MULTIVERSE_DIR}/../multiverse_ws2/install/multiverse_control/lib/libmultiverse_client_json.so ) # # EXPORTS