Skip to content

Commit

Permalink
Fix bug for ros2_control
Browse files Browse the repository at this point in the history
  • Loading branch information
@HoangGiang93
HoangGiang93 committed Aug 9, 2024
1 parent daaa0e9 commit 28522d2
Show file tree
Hide file tree
Showing 3 changed files with 49 additions and 49 deletions.
8 changes: 4 additions & 4 deletions multiverse/modules/multiverse_connectors/scripts/launch_ros.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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")
Expand All @@ -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)

Expand Down Expand Up @@ -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)

Expand Down Expand Up @@ -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))
Expand Down
88 changes: 44 additions & 44 deletions multiverse/resources/muv/pr2_ros2_control.muv
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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:
Expand Down Expand Up @@ -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:
Expand Down
2 changes: 1 addition & 1 deletion multiverse_ws2/src/multiverse_core/multiverse_control/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down

0 comments on commit 28522d2

Please sign in to comment.