Unverified Commit 046ce0c5 authored by Denis Štogl's avatar Denis Štogl Committed by GitHub
Browse files

Add corrections and extensions for the rrbot (#71)



* Add corrections and extensions for the rrbot

* Correct build and remove demo nodes from readme.

* Apply suggestions from code review

Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>

* Adjust colors of the robot

* Update ros2_control_demo_robot/package.xml

Co-authored-by: default avatarVictor Lopez <3469405+v-lopez@users.noreply.github.com>

* Update ros2_control_demo_robot/launch/test_rrbot_description.launch.py

Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>
Co-authored-by: default avatarVictor Lopez <3469405+v-lopez@users.noreply.github.com>
parent 620b703e
# ROS2 Control Demos # ros2_control Demos
[![Build Status](https://github.com/ros-controls/ros2_control_demos/workflows/CI/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ACI) [![Build Status](https://github.com/ros-controls/ros2_control_demos/workflows/CI/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ACI)
[![Linters Status](https://github.com/ros-controls/ros2_control_demos/workflows/Linters/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ALinters) [![Linters Status](https://github.com/ros-controls/ros2_control_demos/workflows/Linters/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ALinters)
[![Coverage Status](https://github.com/ros-controls/ros2_control_demos/workflows/Coverage/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ACoverage) [![Coverage Status](https://github.com/ros-controls/ros2_control_demos/workflows/Coverage/badge.svg?branch=master)](https://github.com/ros-controls/ros2_control_demos/actions?query=workflow%3ACoverage)
[![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0) [![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
This repository provides templates for the development of `ros2_control`-enabled robots and a simple simulation of a robot. This repository provides templates for the development of `ros2_control`-enabled robots and a simple simulations to demonstrate and prove `ros2_control` concepts.
## Goals ## Goals
...@@ -46,10 +46,15 @@ These are some quick hints, especially for those coming from a ROS1 control back ...@@ -46,10 +46,15 @@ These are some quick hints, especially for those coming from a ROS1 control back
* <ros2_control> tags in the URDF must be compatible with the controller's configuration. * <ros2_control> tags in the URDF must be compatible with the controller's configuration.
* PLUGINLIB_EXPORT_CLASS macro is required when implementing an interface. * PLUGINLIB_EXPORT_CLASS macro is required when implementing an interface.
# Test of the Scenario Before the First Release # Build from source
* Checkout [ros-controls/ros2_control](https://github.com/ros-controls/ros2_control) to get the core. ```
* Checkout [ros-controls/ros2_controllers](https://github.com/ros-controls/ros2_controllers) to get all the controllers. git clone https://github.com/ros-controls/ros2_control
* Checkout [ros-controls/ros2_control_demos](https://github.com/ros-controls/ros2_control_demos) to get example hardware and robot launch files. git clone https://github.com/ros-controls/ros2_controllers
git clone https://github.com/ros-controls/ros2_control_demos
```
**NOTE**: `ros2_control` and `ros2_controllers` packages are released for foxy and can be installed using package manager.
For daily use it is recommended to use the released version but there may always be some not-yet-released changes that are required to build the demos.
* Install dependencies (maybe you need `sudo`): * Install dependencies (maybe you need `sudo`):
``` ```
...@@ -64,80 +69,125 @@ These are some quick hints, especially for those coming from a ROS1 control back ...@@ -64,80 +69,125 @@ These are some quick hints, especially for those coming from a ROS1 control back
* Do not forget to source `setup.bash` from the `install` folder! * Do not forget to source `setup.bash` from the `install` folder!
# Getting Started with ROS2 Control # Getting Started with ros2_control
Each of the described example cases from the roadmap has its own launch and URDF file. Each of the described example cases from the roadmap has its own launch and URDF file.
## Example 1: "Industrial Robots with only one interface" ## Starting example robots
1. Start the roslaunch file: Each example is started with a single launch file which starts up the robot hardware, loads controller configurations and it also opens `rviz2`.
```
ros2 launch ros2_control_demo_robot rrbot_system_position_only.launch.py The `rviz2` setup can be recreated following these steps:
```
- The robot models can be visualized using `RobotModel` display using `/robot_description` topic.
2. Open another terminal and check if `RRBotSystemPositionOnlyHardware` is loaded properly: - Or you can simply open the configuration from `rviz` folder in `ros2_control_demo_robot` package manually or directly by executing:
``` ```
ros2 control list_hardware_interfaces rviz2 --display-config `ros2 pkg prefix ros2_control_demo_robot`/share/ros2_control_demo_robot/rviz/rrbot.rviz
``` ```
You should get something like:
``` *RRBot*, or ''Revolute-Revolute Manipulator Robot'', is a simple 3-linkage, 2-joint arm that we will use to demonstrate various features. It essentially a double inverted pendulum and demonstrates some fun control concepts within a simulator and was originally introduced for Gazebo tutorials.
command interfaces The *RRbot* URDF files can be found in the `description` folder of `ros2_control_demo_robot` package.
joint1/position [unclaimed]
joint2/position [unclaimed] ### Example 1: "Industrial Robots with only one interface"
state interfaces
joint1/position 1. Open another terminal and start the roslaunch file:
joint2/position ```
``` ros2 launch ros2_control_demo_robot rrbot_system_position_only.launch.py
```
3. Open another terminal and load, configure, and start controllers:
``` 2. Open another terminal and check that `RRBotSystemPositionOnlyHardware` loaded properly:
ros2 control load_start_controller joint_state_controller ```
ros2 control load_configure_controller forward_command_controller_position ros2 control list_hardware_interfaces
``` ```
You should get something like:
Check if the controller is loaded properly: ```
``` command interfaces
ros2 control list_controllers joint1/position [unclaimed]
``` joint2/position [unclaimed]
You should get the response: state interfaces
``` joint1/position
joint_state_controller[joint_state_controller/JointStateController] active joint2/position
forward_command_controller_position[forward_command_controller/ForwardCommandController] inactive ```
```
## Controlles and moving hardware
4. Starting controller: To move the robot you should load and start controllers.
``` The `JointStateController` is used to publish the joint states to ROS topics.
ros2 control switch_controllers --start-controllers forward_command_controller_position Direct joint commands are sent to this robot via the `ForwardCommandController`.
``` The sections below describe their usage.
Check the [Results](##result) section on how to ensure that things went well.
Check if controllers are activated:
``` ### JointStateController
ros2 control list_controllers
``` Open another terminal and load, configure and start `joint_state_controller`:
You should get `active` in the response: ```
``` ros2 control load_start_controller joint_state_controller
joint_state_controller[joint_state_controller/JointStateController] active ```
forward_command_controller_position[forward_command_controller/ForwardCommandController] active Check if controller is loaded properly:
``` ```
ros2 control list_controllers
5. Open another terminal and send a message to the controller: ```
``` You should get the response:
ros2 topic pub /forward_command_controller_position/commands std_msgs/msg/Float64MultiArray "data: ```
- 0.5 joint_state_controller[joint_state_controller/JointStateController] active
- 0.5" ```
```
Now you should also see the *RRbot* represented correctly in `rviz2`.
You should see how the example output changes. Look for the following lines
```
[RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 0! ### Using ForwardCommandController
[RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 1!
``` 1. If you want to test hardware with `ForwardCommandController` first load and configure it:
```
If you echo the `/joint_states` or `/dynamic_joint_states` topics you should also get similar values. ros2 control load_configure_controller forward_position_controller
``` ```
ros2 topic echo /joint_states Check if the controller is loaded properly:
ros2 topic echo /dynamic_joint_states ```
``` ros2 control list_controllers
```
The other launch-files have corresponding names to their coresponding example. You should get the response:
The URDF files can be found in the `description` folder. ```
joint_state_controller[joint_state_controller/JointStateController] active
forward_position_controller[forward_command_controller/ForwardCommandController] inactive
```
2. Now start the controller:
```
ros2 control switch_controllers --start-controllers forward_position_controller
```
Check if controllers are activated:
```
ros2 control list_controllers
```
You should get `active` in the response:
```
joint_state_controller[joint_state_controller/JointStateController] active
forward_position_controller[forward_command_controller/ForwardCommandController] active
```
**NOTE**: You can do this in only one step by using `load_start_controller` verb instead of `load_configure_controller`.
3. Send command to the controller, either:
a. Manually using ros2 cli interface:
```
ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data:
- 0.5
- 0.5"
```
## Result
1. Independently from the controller you should see how the example's output changes.
Look for the following lines
```
[RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 0!
[RRBotSystemPositionOnlyHardware]: Got state 0.0 for joint 1!
```
2. If you echo the `/joint_states` or `/dynamic_joint_states` topics you should also get similar values.
```
ros2 topic echo /joint_states
ros2 topic echo /dynamic_joint_states
```
3. You should also see the *RRbot* moving in `rviz2`.
...@@ -20,18 +20,15 @@ ...@@ -20,18 +20,15 @@
#include <string> #include <string>
#include <vector> #include <vector>
#include "rclcpp/macros.hpp"
#include "hardware_interface/base_interface.hpp" #include "hardware_interface/base_interface.hpp"
#include "hardware_interface/system_interface.hpp" #include "hardware_interface/system_interface.hpp"
#include "hardware_interface/handle.hpp" #include "hardware_interface/handle.hpp"
#include "hardware_interface/hardware_info.hpp" #include "hardware_interface/hardware_info.hpp"
#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "hardware_interface/types/hardware_interface_return_values.hpp"
#include "hardware_interface/types/hardware_interface_status_values.hpp" #include "hardware_interface/types/hardware_interface_status_values.hpp"
#include "rclcpp/macros.hpp"
#include "ros2_control_demo_hardware/visibility_control.h" #include "ros2_control_demo_hardware/visibility_control.h"
using hardware_interface::return_type;
namespace ros2_control_demo_hardware namespace ros2_control_demo_hardware
{ {
class RRBotSystemPositionOnlyHardware : public class RRBotSystemPositionOnlyHardware : public
...@@ -41,7 +38,7 @@ public: ...@@ -41,7 +38,7 @@ public:
RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware); RCLCPP_SHARED_PTR_DEFINITIONS(RRBotSystemPositionOnlyHardware);
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type configure(const hardware_interface::HardwareInfo & info) override; hardware_interface::return_type configure(const hardware_interface::HardwareInfo & info) override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
std::vector<hardware_interface::StateInterface> export_state_interfaces() override; std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
...@@ -50,16 +47,16 @@ public: ...@@ -50,16 +47,16 @@ public:
std::vector<hardware_interface::CommandInterface> export_command_interfaces() override; std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type start() override; hardware_interface::return_type start() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type stop() override; hardware_interface::return_type stop() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type read() override; hardware_interface::return_type read() override;
ROS2_CONTROL_DEMO_HARDWARE_PUBLIC ROS2_CONTROL_DEMO_HARDWARE_PUBLIC
return_type write() override; hardware_interface::return_type write() override;
private: private:
// Parameters for the RRBot simulation // Parameters for the RRBot simulation
......
...@@ -26,11 +26,11 @@ ...@@ -26,11 +26,11 @@
namespace ros2_control_demo_hardware namespace ros2_control_demo_hardware
{ {
return_type RRBotSystemPositionOnlyHardware::configure( hardware_interface::return_type RRBotSystemPositionOnlyHardware::configure(
const hardware_interface::HardwareInfo & info) const hardware_interface::HardwareInfo & info)
{ {
if (configure_default(info) != return_type::OK) { if (configure_default(info) != hardware_interface::return_type::OK) {
return return_type::ERROR; return hardware_interface::return_type::ERROR;
} }
hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]); hw_start_sec_ = stod(info_.hardware_parameters["example_param_hw_start_duration_sec"]);
...@@ -46,7 +46,7 @@ return_type RRBotSystemPositionOnlyHardware::configure( ...@@ -46,7 +46,7 @@ return_type RRBotSystemPositionOnlyHardware::configure(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' has %d command interfaces found. 1 expected.", "Joint '%s' has %d command interfaces found. 1 expected.",
joint.name.c_str(), joint.command_interfaces.size()); joint.name.c_str(), joint.command_interfaces.size());
return return_type::ERROR; return hardware_interface::return_type::ERROR;
} }
if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
...@@ -55,7 +55,7 @@ return_type RRBotSystemPositionOnlyHardware::configure( ...@@ -55,7 +55,7 @@ return_type RRBotSystemPositionOnlyHardware::configure(
"Joint '%s' have %s command interfaces found. '%s' expected.", "Joint '%s' have %s command interfaces found. '%s' expected.",
joint.name.c_str(), joint.command_interfaces[0].name.c_str(), joint.name.c_str(), joint.command_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION); hardware_interface::HW_IF_POSITION);
return return_type::ERROR; return hardware_interface::return_type::ERROR;
} }
if (joint.state_interfaces.size() != 1) { if (joint.state_interfaces.size() != 1) {
...@@ -63,7 +63,7 @@ return_type RRBotSystemPositionOnlyHardware::configure( ...@@ -63,7 +63,7 @@ return_type RRBotSystemPositionOnlyHardware::configure(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joint '%s' has %d state interface. 1 expected.", "Joint '%s' has %d state interface. 1 expected.",
joint.name.c_str(), joint.state_interfaces.size()); joint.name.c_str(), joint.state_interfaces.size());
return return_type::ERROR; return hardware_interface::return_type::ERROR;
} }
if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) { if (joint.state_interfaces[0].name != hardware_interface::HW_IF_POSITION) {
...@@ -72,12 +72,12 @@ return_type RRBotSystemPositionOnlyHardware::configure( ...@@ -72,12 +72,12 @@ return_type RRBotSystemPositionOnlyHardware::configure(
"Joint '%s' have %s state interface. '%s' expected.", "Joint '%s' have %s state interface. '%s' expected.",
joint.name.c_str(), joint.state_interfaces[0].name.c_str(), joint.name.c_str(), joint.state_interfaces[0].name.c_str(),
hardware_interface::HW_IF_POSITION); hardware_interface::HW_IF_POSITION);
return return_type::ERROR; return hardware_interface::return_type::ERROR;
} }
} }
status_ = hardware_interface::status::CONFIGURED; status_ = hardware_interface::status::CONFIGURED;
return return_type::OK; return hardware_interface::return_type::OK;
} }
std::vector<hardware_interface::StateInterface> std::vector<hardware_interface::StateInterface>
...@@ -107,7 +107,7 @@ RRBotSystemPositionOnlyHardware::export_command_interfaces() ...@@ -107,7 +107,7 @@ RRBotSystemPositionOnlyHardware::export_command_interfaces()
} }
return_type RRBotSystemPositionOnlyHardware::start() hardware_interface::return_type RRBotSystemPositionOnlyHardware::start()
{ {
RCLCPP_INFO( RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
...@@ -120,11 +120,13 @@ return_type RRBotSystemPositionOnlyHardware::start() ...@@ -120,11 +120,13 @@ return_type RRBotSystemPositionOnlyHardware::start()
"%.1f seconds left...", hw_start_sec_ - i); "%.1f seconds left...", hw_start_sec_ - i);
} }
// set some default values // set some default values when starting the first time
for (uint i = 0; i < hw_states_.size(); i++) { for (uint i = 0; i < hw_states_.size(); i++) {
if (std::isnan(hw_states_[i])) { if (std::isnan(hw_states_[i])) {
hw_states_[i] = 0; hw_states_[i] = 0;
hw_commands_[i] = 0; hw_commands_[i] = 0;
} else {
hw_commands_[i] = hw_states_[i];
} }
} }
...@@ -134,10 +136,10 @@ return_type RRBotSystemPositionOnlyHardware::start() ...@@ -134,10 +136,10 @@ return_type RRBotSystemPositionOnlyHardware::start()
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"System Sucessfully started!"); "System Sucessfully started!");
return return_type::OK; return hardware_interface::return_type::OK;
} }
return_type RRBotSystemPositionOnlyHardware::stop() hardware_interface::return_type RRBotSystemPositionOnlyHardware::stop()
{ {
RCLCPP_INFO( RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
...@@ -156,7 +158,7 @@ return_type RRBotSystemPositionOnlyHardware::stop() ...@@ -156,7 +158,7 @@ return_type RRBotSystemPositionOnlyHardware::stop()
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"System sucessfully stopped!"); "System sucessfully stopped!");
return return_type::OK; return hardware_interface::return_type::OK;
} }
hardware_interface::return_type RRBotSystemPositionOnlyHardware::read() hardware_interface::return_type RRBotSystemPositionOnlyHardware::read()
...@@ -167,7 +169,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read() ...@@ -167,7 +169,7 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read()
for (uint i = 0; i < hw_states_.size(); i++) { for (uint i = 0; i < hw_states_.size(); i++) {
// Simulate RRBot's movement // Simulate RRBot's movement
hw_states_[i] = hw_commands_[i] + (hw_states_[i] - hw_commands_[i]) / hw_slowdown_; hw_states_[i] = hw_states_[i] + (hw_commands_[i] - hw_states_[i]) / hw_slowdown_;
RCLCPP_INFO( RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Got state %.5f for joint %d!", hw_states_[i], i); "Got state %.5f for joint %d!", hw_states_[i], i);
...@@ -176,10 +178,10 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read() ...@@ -176,10 +178,10 @@ hardware_interface::return_type RRBotSystemPositionOnlyHardware::read()
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joints sucessfully read!"); "Joints sucessfully read!");
return return_type::OK; return hardware_interface::return_type::OK;
} }
hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemPositionOnlyHardware::write() hardware_interface::return_type RRBotSystemPositionOnlyHardware::write()
{ {
RCLCPP_INFO( RCLCPP_INFO(
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
...@@ -195,7 +197,7 @@ hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemPositionO ...@@ -195,7 +197,7 @@ hardware_interface::return_type ros2_control_demo_hardware::RRBotSystemPositionO
rclcpp::get_logger("RRBotSystemPositionOnlyHardware"), rclcpp::get_logger("RRBotSystemPositionOnlyHardware"),
"Joints sucessfully written!"); "Joints sucessfully written!");
return return_type::OK; return hardware_interface::return_type::OK;
} }
} // namespace ros2_control_demo_hardware } // namespace ros2_control_demo_hardware
......
...@@ -15,7 +15,7 @@ find_package(ament_cmake REQUIRED) ...@@ -15,7 +15,7 @@ find_package(ament_cmake REQUIRED)
## INSTALL ## INSTALL
install( install(
DIRECTORY controllers description launch/ DIRECTORY config description launch rviz
DESTINATION share/${PROJECT_NAME} DESTINATION share/${PROJECT_NAME}
) )
......
...@@ -2,13 +2,16 @@ controller_manager: ...@@ -2,13 +2,16 @@ controller_manager:
ros__parameters: ros__parameters:
update_rate: 2 # Hz update_rate: 2 # Hz
forward_command_controller_position:
type: forward_command_controller/ForwardCommandController
joint_state_controller: joint_state_controller:
type: joint_state_controller/JointStateController type: joint_state_controller/JointStateController
forward_command_controller_position: forward_position_controller:
type: forward_command_controller/ForwardCommandController
position_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
forward_position_controller:
ros__parameters: ros__parameters:
joints: joints:
- joint1 - joint1
......
...@@ -33,6 +33,10 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc ...@@ -33,6 +33,10 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
<color rgba="0.8 0.0 0.0 1.0"/> <color rgba="0.8 0.0 0.0 1.0"/>
</material> </material>
<material name="yellow">
<color rgba="1.0 1.0 0.0 1.0"/>
</material>
<material name="white"> <material name="white">
<color rgba="1.0 1.0 1.0 1.0"/> <color rgba="1.0 1.0 1.0 1.0"/>
</material> </material>
......
...@@ -4,10 +4,9 @@ ...@@ -4,10 +4,9 @@
<xacro:macro name="rrbot" params="parent prefix *origin"> <xacro:macro name="rrbot" params="parent prefix *origin">
<!-- Constants for robot dimensions --> <!-- Constants for robot dimensions -->
<xacro:property name="PI" value="3.1415926535897931"/>
<xacro:property name="mass" value="1" /> <!-- arbitrary value for mass --> <xacro:property name="mass" value="1" /> <!-- arbitrary value for mass -->
<xacro:property name="width" value="0.1" /> <!-- Square dimensions (widthxwidth) of beams --> <xacro:property name="width" value="0.1" /> <!-- Square dimensions (widthxwidth) of beams -->
<xacro:property name="height1" value="2" /> <!-- Link 1 --> <xacro:property name="height1" value="0.2" /> <!-- Link 1 -->
<xacro:property name="height2" value="1" /> <!-- Link 2 --> <xacro:property name="height2" value="1" /> <!-- Link 2 -->
<xacro:property name="height3" value="1" /> <!-- Link 3 --> <xacro:property name="height3" value="1" /> <!-- Link 3 -->
<xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box --> <xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
...@@ -68,7 +67,7 @@ ...@@ -68,7 +67,7 @@
<geometry> <geometry>
<box size="${width} ${width} ${height2}"/> <box size="${width} ${width} ${height2}"/>
</geometry> </geometry>
<material name="black"/> <material name="yellow"/>
</visual> </visual>
<inertial> <inertial>
......
<?xml version="1.0"?> <?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="rrbot_system_position_only" params="name prefix"> <xacro:macro name="rrbot_system_position_only" params="name prefix slowdown:=2.0">
<ros2