diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..40776c2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +test/__pycache__/ diff --git a/include/topic_based_ros2_control/topic_based_system.hpp b/include/topic_based_ros2_control/topic_based_system.hpp index cf67d61..f300eb0 100644 --- a/include/topic_based_ros2_control/topic_based_system.hpp +++ b/include/topic_based_ros2_control/topic_based_system.hpp @@ -80,6 +80,8 @@ class TopicBasedSystem : public hardware_interface::SystemInterface struct MimicJoint { + std::string joint_name; + std::string mimicked_joint_name; std::size_t joint_index; std::size_t mimicked_joint_index; double multiplier = 1.0; diff --git a/src/topic_based_system.cpp b/src/topic_based_system.cpp index 67a9ec9..274765b 100644 --- a/src/topic_based_system.cpp +++ b/src/topic_based_system.cpp @@ -114,6 +114,8 @@ CallbackReturn TopicBasedSystem::on_init(const hardware_interface::HardwareInfo& throw std::runtime_error(std::string("Mimicked joint '") + joint.parameters.at("mimic") + "' not found"); } MimicJoint mimic_joint; + mimic_joint.joint_name = joint.name; + mimic_joint.mimicked_joint_name = mimicked_joint_it->name; mimic_joint.joint_index = i; mimic_joint.mimicked_joint_index = static_cast(std::distance(info_.joints.begin(), mimicked_joint_it)); @@ -271,64 +273,146 @@ hardware_interface::return_type TopicBasedSystem::write(const rclcpp::Time& /*ti joint_states_[POSITION_INTERFACE_INDEX].cbegin(), joint_states_[POSITION_INTERFACE_INDEX].cend(), joint_commands_[POSITION_INTERFACE_INDEX].cbegin(), 0.0, [](const auto d1, const auto d2) { return std::abs(d1) + std::abs(d2); }, std::minus{}); - if (diff <= trigger_joint_command_threshold_) + + bool exist_velocity_command = false; + static bool exist_velocity_command_old = false; // Use old state to publish zero velocities + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + if (fabs(joint_commands_[VELOCITY_INTERFACE_INDEX][i]) > trigger_joint_command_threshold_) + { + exist_velocity_command = true; + } + } + + if (diff <= trigger_joint_command_threshold_ && + (exist_velocity_command == false && exist_velocity_command_old == false)) { return hardware_interface::return_type::OK; } - sensor_msgs::msg::JointState joint_state; - for (std::size_t i = 0; i < info_.joints.size(); ++i) + exist_velocity_command_old = exist_velocity_command; + + // For Position Joint { - joint_state.name.push_back(info_.joints[i].name); + sensor_msgs::msg::JointState joint_state; joint_state.header.stamp = node_->now(); - // only send commands to the interfaces that are defined for this joint - for (const auto& interface : info_.joints[i].command_interfaces) + for (std::size_t i = 0; i < info_.joints.size(); ++i) { - if (interface.name == hardware_interface::HW_IF_POSITION) + // only send commands to the interfaces that are defined for this joint + for (const auto& interface : info_.joints[i].command_interfaces) { - joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]); + if (interface.name == hardware_interface::HW_IF_POSITION) + { + joint_state.name.push_back(info_.joints[i].name); + joint_state.position.push_back(joint_commands_[POSITION_INTERFACE_INDEX][i]); + } } - else if (interface.name == hardware_interface::HW_IF_VELOCITY) + } + + for (const auto& mimic_joint : mimic_joints_) + { + for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) { - joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]); + if (interface.name == hardware_interface::HW_IF_POSITION) + { + for (size_t index = 0; index < joint_state.name.size(); index++) + { + if (joint_state.name[index] == mimic_joint.mimicked_joint_name) + { + joint_state.name.push_back(mimic_joint.joint_name); + joint_state.position.push_back(mimic_joint.multiplier * joint_state.position[index]); + } + } + } } - else if (interface.name == hardware_interface::HW_IF_EFFORT) + } + + if (rclcpp::ok() && joint_state.name.size() != 0) + { + topic_based_joint_commands_publisher_->publish(joint_state); + } + } + + // For Velocity Joint + { + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->now(); + for (std::size_t i = 0; i < info_.joints.size(); ++i) + { + // only send commands to the interfaces that are defined for this joint + for (const auto& interface : info_.joints[i].command_interfaces) { - joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]); + if (interface.name == hardware_interface::HW_IF_VELOCITY) + { + joint_state.name.push_back(info_.joints[i].name); + joint_state.velocity.push_back(joint_commands_[VELOCITY_INTERFACE_INDEX][i]); + } } - else + } + + for (const auto& mimic_joint : mimic_joints_) + { + for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) { - RCLCPP_WARN_ONCE(node_->get_logger(), "Joint '%s' has unsupported command interfaces found: %s.", - info_.joints[i].name.c_str(), interface.name.c_str()); + if (interface.name == hardware_interface::HW_IF_VELOCITY) + { + for (size_t index = 0; index < joint_state.name.size(); index++) + { + if (joint_state.name[index] == mimic_joint.mimicked_joint_name) + { + joint_state.name.push_back(mimic_joint.joint_name); + joint_state.velocity.push_back(mimic_joint.multiplier * joint_state.velocity[index]); + } + } + } } } + + if (rclcpp::ok() && joint_state.name.size() != 0) + { + topic_based_joint_commands_publisher_->publish(joint_state); + } } - for (const auto& mimic_joint : mimic_joints_) + // For Effort Joint { - for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) + sensor_msgs::msg::JointState joint_state; + joint_state.header.stamp = node_->now(); + for (std::size_t i = 0; i < info_.joints.size(); ++i) { - if (interface.name == hardware_interface::HW_IF_POSITION) - { - joint_state.position[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.position[mimic_joint.mimicked_joint_index]; - } - else if (interface.name == hardware_interface::HW_IF_VELOCITY) + // only send commands to the interfaces that are defined for this joint + for (const auto& interface : info_.joints[i].command_interfaces) { - joint_state.velocity[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.velocity[mimic_joint.mimicked_joint_index]; + if (interface.name == hardware_interface::HW_IF_EFFORT) + { + joint_state.name.push_back(info_.joints[i].name); + joint_state.effort.push_back(joint_commands_[EFFORT_INTERFACE_INDEX][i]); + } } - else if (interface.name == hardware_interface::HW_IF_EFFORT) + } + + for (const auto& mimic_joint : mimic_joints_) + { + for (const auto& interface : info_.joints[mimic_joint.mimicked_joint_index].command_interfaces) { - joint_state.effort[mimic_joint.joint_index] = - mimic_joint.multiplier * joint_state.effort[mimic_joint.mimicked_joint_index]; + if (interface.name == hardware_interface::HW_IF_EFFORT) + { + for (size_t index = 0; index < joint_state.name.size(); index++) + { + if (joint_state.name[index] == mimic_joint.mimicked_joint_name) + { + joint_state.name.push_back(mimic_joint.joint_name); + joint_state.effort.push_back(mimic_joint.multiplier * joint_state.effort[index]); + } + } + } } } - } - if (rclcpp::ok()) - { - topic_based_joint_commands_publisher_->publish(joint_state); + if (rclcpp::ok() && joint_state.name.size() != 0) + { + topic_based_joint_commands_publisher_->publish(joint_state); + } } return hardware_interface::return_type::OK; diff --git a/test/control.launch.py b/test/control.launch.py index 7b44508..ab33e2e 100644 --- a/test/control.launch.py +++ b/test/control.launch.py @@ -41,9 +41,9 @@ def generate_launch_description(): ros2_controllers_file = Path(SCRIPT_PATH / "ros2_controllers.yaml") robot_description = { - "robot_description": xacro.process_file(SCRIPT_PATH / "rrr.urdf.xacro").toxml(), + "robot_description": xacro.process_file(SCRIPT_PATH / "diffbot_with_rrr.urdf.xacro").toxml(), } - controllers = ["joint_state_broadcaster", "joint_trajectory_controller"] + controllers = ["joint_state_broadcaster", "joint_trajectory_controller", "diff_drive_controller", "effort_controller"] return LaunchDescription( [ Node( diff --git a/test/diffbot_with_rrr.urdf.xacro b/test/diffbot_with_rrr.urdf.xacro new file mode 100755 index 0000000..1932667 --- /dev/null +++ b/test/diffbot_with_rrr.urdf.xacro @@ -0,0 +1,275 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + 50 + + + hardware_interface/VelocityJointInterface + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + topic_based_ros2_control/TopicBasedSystem + /topic_based_joint_commands + /topic_based_joint_states + + + + -1 + 1 + + + + + + + + -1 + 1 + + + + + + + + + + + + + + + 0.2 + + + + + + + 0.3 + + + + + + + 0.1 + + + + + + diff --git a/test/ros2_controllers.yaml b/test/ros2_controllers.yaml index acdf09c..708b714 100644 --- a/test/ros2_controllers.yaml +++ b/test/ros2_controllers.yaml @@ -5,6 +5,10 @@ controller_manager: type: joint_state_broadcaster/JointStateBroadcaster joint_trajectory_controller: type: joint_trajectory_controller/JointTrajectoryController + diff_drive_controller: + type: diff_drive_controller/DiffDriveController + effort_controller: + type: effort_controllers/JointGroupEffortController joint_trajectory_controller: ros__parameters: @@ -23,3 +27,51 @@ joint_trajectory_controller: constraints: stopped_velocity_tolerance: 0.0 goal_time: 0.0 + +diff_drive_controller: + ros__parameters: + left_wheel_names : ['left_wheel_joint'] + right_wheel_names : ['right_wheel_joint'] + + wheel_separation : 0.46 + wheel_radius : 0.19 + wheel_separation_multiplier : 1.0 + left_wheel_radius_multiplier : 1.0 + right_wheel_radius_multiplier : 1.0 + + odom_frame_id: odom + base_frame_id: base_link + + pose_covariance_diagonal : [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + twist_covariance_diagonal: [0.001, 0.001, 1000000.0, 1000000.0, 1000000.0, 1000.0] + + enable_odom_tf: true + + cmd_vel_timeout: 3.0 + publish_limited_velocity: true + velocity_rolling_window_size: 10 + + # limits + linear.x.has_velocity_limits: true + linear.x.has_acceleration_limits: false + linear.x.has_jerk_limits: false + linear.x.max_velocity: 1.0 + linear.x.min_velocity: -1.0 + linear.x.max_acceleration: 0.4 + linear.x.min_acceleration: -0.4 + linear.x.max_jerk: 0.5 + linear.x.min_jerk: -0.5 + + angular.z.has_velocity_limits: true + angular.z.has_acceleration_limits: false + angular.z.has_jerk_limits: false + angular.z.max_velocity: 1.5 + angular.z.min_velocity: -1.5 + angular.z.max_acceleration: 0.8 + angular.z.min_acceleration: -0.8 + angular.z.max_jerk: 0.5 + angular.z.min_jerk: -0.5 + +effort_controller: + ros__parameters: + joints: ['rrr_base_joint'] diff --git a/test/rrr.urdf.xacro b/test/rrr.urdf.xacro deleted file mode 100644 index 26a9c13..0000000 --- a/test/rrr.urdf.xacro +++ /dev/null @@ -1,140 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - topic_based_ros2_control/TopicBasedSystem - /topic_based_joint_commands - /topic_based_joint_states - - - - - 0.2 - - - - - - - 0.3 - - - - - - - 0.1 - - - - - diff --git a/test/test_topic_based_robot.py b/test/test_topic_based_robot.py index 7f2b600..e5e7339 100755 --- a/test/test_topic_based_robot.py +++ b/test/test_topic_based_robot.py @@ -35,16 +35,28 @@ rclpy.init() -robot = TopicBasedRobot(["joint_1", "joint_2", "joint_3"]) +robot = TopicBasedRobot(["joint_1", "joint_2", "joint_3"], ["left_wheel_joint", "right_wheel_joint"], ["rrr_base_joint"]) # By default the joint_states should have the values from initial_value from rrr.urdf.xacro -current_joint_state = robot.get_current_joint_state() +current_joint_state = robot.get_current_position_joint_state() urdf_initial_values = [0.2, 0.3, 0.1] assert ( current_joint_state == urdf_initial_values ), f"{current_joint_state=} != {urdf_initial_values=}" -# Test setting the robot joint states +# Test setting the robot joint position states joint_state = [0.1, 0.2, 0.3] robot.set_joint_positions(joint_state) -current_joint_state = robot.get_current_joint_state() +current_joint_state = robot.get_current_position_joint_state() +assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" + +# Test setting the robot joint velocity states +joint_state = [0.4, 0.5] +robot.set_joint_velocities(joint_state) +current_joint_state = robot.get_current_velocity_joint_state() +assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" + +# Test setting the robot joint effort states +joint_state = [0.6] +robot.set_joint_efforts(joint_state) +current_joint_state = robot.get_current_effort_joint_state() assert current_joint_state == joint_state, f"{current_joint_state=} != {joint_state=}" diff --git a/test/topic_based_robot.py b/test/topic_based_robot.py index fc33a83..9267160 100644 --- a/test/topic_based_robot.py +++ b/test/topic_based_robot.py @@ -42,9 +42,11 @@ class TopicBasedRobot(Node): """Topic Based Robot to test topic_based_ros2_control interface""" - def __init__(self, joint_names: list[str]) -> None: + def __init__(self, position_joint_names: list[str], velocity_joint_names = [], effort_joint_names = []) -> None: super().__init__("topic_based_robot") - self.joint_names = joint_names.copy() + self.position_joint_names = position_joint_names.copy() + self.velocity_joint_names = velocity_joint_names.copy() + self.effort_joint_names = effort_joint_names.copy() self.last_joint_command = [] self.current_joint_state = [] self.callback_group = ReentrantCallbackGroup() @@ -72,46 +74,139 @@ def __init__(self, joint_names: list[str]) -> None: callback_group=self.callback_group, ) - def filter_joint_state_msg(self, msg: JointState): + def filter_joint_position_state_msg(self, msg: JointState): joint_states = [] - for joint_name in self.joint_names: + self.get_logger().warning( + f"Caught msg name list: '{msg.name}'", + ) + for joint_name in self.position_joint_names: try: index = msg.name.index(joint_name) except ValueError: - msg = f"Joint name '{joint_name}' not in input keys {msg.name}" - raise ValueError(msg) from None + continue joint_states.append(msg.position[index]) return joint_states + def filter_joint_velocity_state_msg(self, msg: JointState): + joint_states = [] + self.get_logger().warning( + f"Caught msg name list: '{msg.name}'", + ) + for joint_name in self.velocity_joint_names: + try: + index = msg.name.index(joint_name) + except ValueError: + continue + joint_states.append(msg.velocity[index]) + return joint_states + + def filter_joint_effort_state_msg(self, msg: JointState): + joint_states = [] + self.get_logger().warning( + f"Caught msg name list: '{msg.name}'", + ) + for joint_name in self.effort_joint_names: + try: + index = msg.name.index(joint_name) + except ValueError: + continue + joint_states.append(msg.effort[index]) + return joint_states + def command_callback(self, msg: JointState): - self.last_joint_command = self.filter_joint_state_msg(msg) + # Output by position, velocity, and force, so the number of joint names of each type is matched + if self.position_joint_names[0] in msg.name: + assert len(self.position_joint_names) == len(msg.name), f"{self.position_joint_names=} != {msg.name=}" + self.last_position_joint_command = self.filter_joint_position_state_msg(msg) + + if not len(self.velocity_joint_names) == 0: + if self.velocity_joint_names[0] in msg.name: + assert len(self.velocity_joint_names) == len(msg.name), f"{self.velocity_joint_names=} != {msg.name=}" + self.last_velocity_joint_command = self.filter_joint_velocity_state_msg(msg) + + if not len(self.effort_joint_names) == 0: + if self.effort_joint_names[0] in msg.name: + assert len(self.effort_joint_names) == len(msg.name), f"{self.effort_joint_names=} != {msg.name=}" + self.last_effort_joint_command = self.filter_joint_effort_state_msg(msg) def joint_states_callback(self, msg: JointState): - self.current_joint_state = self.filter_joint_state_msg(msg) + self.current_position_joint_state = self.filter_joint_position_state_msg(msg) + if not len(self.velocity_joint_names) == 0: + self.current_velocity_joint_state = self.filter_joint_velocity_state_msg(msg) + if not len(self.effort_joint_names) == 0: + self.current_effort_joint_state = self.filter_joint_effort_state_msg(msg) - def get_current_joint_command(self) -> OrderedDict[str, float]: + def get_current_position_joint_command(self) -> OrderedDict[str, float]: """Get the last joint command sent to the robot.""" - self.last_joint_command = [] - while len(self.last_joint_command) == 0: + self.last_position_joint_command = [] + while len(self.last_position_joint_command) == 0: self.get_logger().warning( f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", throttle_duration_sec=2.0, skip_first=True, ) rclpy.spin_once(self, timeout_sec=0.0) - return self.last_joint_command + return self.last_position_joint_command - def get_current_joint_state(self) -> OrderedDict[str, float]: + def get_current_velocity_joint_command(self) -> OrderedDict[str, float]: + """Get the last joint command sent to the robot.""" + self.last_velocity_joint_command = [] + while len(self.last_velocity_joint_command) == 0: + self.get_logger().warning( + f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.last_velocity_joint_command + + def get_current_effort_joint_command(self) -> OrderedDict[str, float]: + """Get the last joint command sent to the robot.""" + self.last_effort_joint_command = [] + while len(self.last_effort_joint_command) == 0: + self.get_logger().warning( + f"Waiting for joint command '{self.desired_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.last_effort_joint_command + + def get_current_position_joint_state(self) -> OrderedDict[str, float]: """Get the current joint state reported by ros2_control on joint_states topic.""" - self.current_joint_state = [] - while len(self.current_joint_state) == 0: + self.current_position_joint_state = [] + while len(self.current_position_joint_state) == 0: self.get_logger().warning( f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", throttle_duration_sec=2.0, skip_first=True, ) rclpy.spin_once(self, timeout_sec=0.0) - return self.current_joint_state + return self.current_position_joint_state + + def get_current_velocity_joint_state(self) -> OrderedDict[str, float]: + """Get the current joint state reported by ros2_control on joint_states topic.""" + self.current_velocity_joint_state = [] + while len(self.current_velocity_joint_state) == 0: + self.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.current_velocity_joint_state + + def get_current_effort_joint_state(self) -> OrderedDict[str, float]: + """Get the current joint state reported by ros2_control on joint_states topic.""" + self.current_effort_joint_state = [] + while len(self.current_effort_joint_state) == 0: + self.get_logger().warning( + f"Waiting for current joint states from topic '{self.current_joint_state_subscriber.topic_name}'...", + throttle_duration_sec=2.0, + skip_first=True, + ) + rclpy.spin_once(self, timeout_sec=0.0) + return self.current_effort_joint_state def set_joint_positions( self, @@ -120,13 +215,49 @@ def set_joint_positions( """Set the joint positions of the robot.""" self.actual_joint_state_publisher.publish( JointState( - name=list(self.joint_names), + name=list(self.position_joint_names), position=joint_positions, ), ) while not np.allclose( - self.get_current_joint_state(), + self.get_current_position_joint_state(), joint_positions, atol=1e-3, ): rclpy.spin_once(self, timeout_sec=0.0) + + def set_joint_velocities( + self, + joint_velocities: list[float] | np.ndarray, + ) -> None: + """Set the joint velocities of the robot.""" + self.actual_joint_state_publisher.publish( + JointState( + name=list(self.velocity_joint_names), + velocity=joint_velocities, + ), + ) + while not np.allclose( + self.get_current_velocity_joint_state(), + joint_velocities, + atol=1e-3, + ): + rclpy.spin_once(self, timeout_sec=0.0) + + def set_joint_efforts( + self, + joint_efforts: list[float] | np.ndarray, + ) -> None: + """Set the joint efforts of the robot.""" + self.actual_joint_state_publisher.publish( + JointState( + name=list(self.effort_joint_names), + effort=joint_efforts, + ), + ) + while not np.allclose( + self.get_current_effort_joint_state(), + joint_efforts, + atol=1e-3, + ): + rclpy.spin_once(self, timeout_sec=0.0)