diff --git a/doc/index.rst b/doc/index.rst index facbb538d..0837344a5 100644 --- a/doc/index.rst +++ b/doc/index.rst @@ -71,7 +71,7 @@ Example 10: "GPIO interfaces" Industrial robot with GPIO interfaces Example 11: "CarlikeBot" - *CarlikeBot* with an Ackermann steering controller + *CarlikeBot* with a bicycle steering controller Example 12: "Controller chaining" The example shows a simple chainable controller and its integration to form a controller chain to control the joints of *RRBot*. diff --git a/example_11/README.md b/example_11/README.md index 841051c0b..23e88cf84 100644 --- a/example_11/README.md +++ b/example_11/README.md @@ -1,6 +1,6 @@ # ros2_control_demo_example_11 - *CarlikeBot*, or ''Carlike Mobile Robot'', is a simple mobile base with [ackermann drive](https://en.wikipedia.org/wiki/Ackermann_steering_geometry). - The robot has two wheels in the front that steer the robot and two wheels in the back that power the robot forward and backwards. + *CarlikeBot*, or ''Carlike Mobile Robot'', is a simple mobile base with bicycle drive. + The robot has two wheels in the front that steer the robot and two wheels in the back that power the robot forward and backwards. However, since each pair of wheels (steering and traction) are controlled by one interface, a bicycle steering model is used. Find the documentation in [doc/userdoc.rst](doc/userdoc.rst) or on [control.ros.org](https://control.ros.org/master/doc/ros2_control_demos/example_11/doc/userdoc.html). diff --git a/example_11/doc/userdoc.rst b/example_11/doc/userdoc.rst index d6d3edfe7..8f8880b83 100644 --- a/example_11/doc/userdoc.rst +++ b/example_11/doc/userdoc.rst @@ -128,4 +128,4 @@ Controllers from this demo -------------------------- * ``Joint State Broadcaster`` (`ros2_controllers repository `__): `doc `__ -* ``Bicycle Steering Controller`` (`ros2_controllers repository `__): `doc `__ +* ``Bicycle Steering Controller`` (`ros2_controllers repository `__): `doc `__ diff --git a/example_11/hardware/carlikebot_system.cpp b/example_11/hardware/carlikebot_system.cpp index bb3e12bae..b91abc036 100644 --- a/example_11/hardware/carlikebot_system.cpp +++ b/example_11/hardware/carlikebot_system.cpp @@ -281,18 +281,21 @@ hardware_interface::return_type CarlikeBotSystemHardware::read( const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (auto & joint : hw_interfaces_) - { - if (joint.first == "steering") - { - joint.second.state.position = joint.second.command.position; - } - else if (joint.first == "traction") - { - joint.second.state.velocity = joint.second.command.velocity; - joint.second.state.position += joint.second.state.velocity * period.seconds(); - } - } + + hw_interfaces_["steering"].state.position = hw_interfaces_["steering"].command.position; + + hw_interfaces_["traction"].state.velocity = hw_interfaces_["traction"].command.velocity; + hw_interfaces_["traction"].state.position += + hw_interfaces_["traction"].state.velocity * period.seconds(); + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got position state: %.2f for joint '%s'.", + hw_interfaces_["steering"].command.position, hw_interfaces_["steering"].joint_name.c_str()); + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got velocity state: %.2f for joint '%s'.", + hw_interfaces_["traction"].command.velocity, hw_interfaces_["traction"].joint_name.c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; @@ -302,23 +305,15 @@ hardware_interface::return_type ros2_control_demo_example_11 ::CarlikeBotSystemH const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // BEGIN: This part here is for exemplary purposes - Please do not copy to your production code - for (auto & joint : hw_interfaces_) - { - if (joint.first == "steering") - { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Got position command: %.2f for joint '%s'.", joint.second.command.position, - joint.second.joint_name.c_str()); - } - else if (joint.first == "traction") - { - RCLCPP_INFO( - rclcpp::get_logger("CarlikeBotSystemHardware"), - "Got velocity command: %.2f for joint '%s'.", joint.second.command.velocity, - joint.second.joint_name.c_str()); - } - } + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got position command: %.2f for joint '%s'.", + hw_interfaces_["steering"].command.position, hw_interfaces_["steering"].joint_name.c_str()); + + RCLCPP_INFO( + rclcpp::get_logger("CarlikeBotSystemHardware"), "Got velocity command: %.2f for joint '%s'.", + hw_interfaces_["traction"].command.velocity, hw_interfaces_["traction"].joint_name.c_str()); + // END: This part here is for exemplary purposes - Please do not copy to your production code return hardware_interface::return_type::OK; diff --git a/example_11/package.xml b/example_11/package.xml index c690fb943..70391c873 100644 --- a/example_11/package.xml +++ b/example_11/package.xml @@ -28,6 +28,7 @@ robot_state_publisher ros2_controllers_test_nodes ros2_control_demo_description + topic_tools ros2controlcli ros2launch rviz2 diff --git a/example_11/ros2_control_demo_example_11.xml b/example_11/ros2_control_demo_example_11.xml index 40fac739e..92c7fdb0e 100644 --- a/example_11/ros2_control_demo_example_11.xml +++ b/example_11/ros2_control_demo_example_11.xml @@ -3,7 +3,7 @@ type="ros2_control_demo_example_11::CarlikeBotSystemHardware" base_class_type="hardware_interface::SystemInterface"> - The ros2_control CarlikeBot example using a system hardware interface-type. It uses velocity command and state interface for the drive motor and position command and state interface for the steering. The example is the starting point to implement a hardware interface for ackermann and bicycle drive robots. + The ros2_control CarlikeBot example using a system hardware interface-type. It uses velocity and position command and state interface for the drive motor and position command and state interface for the steering. The example is the starting point to implement a hardware interface for ackermann and bicycle drive robots.