How to create a robot system with ros2_control
This guide describes how to create a simple robot using the robot system hardware interface leveragin ros2_control.
Create a new configuration package with the name
canopen_robot_control_example
.
$ ros2 pkg create --dependencies canopen lely_core_libraries --build-type ament_cmake {package_name} $ cd {package_name} $ rm -rf src $ rm -rf include $ mkdir -p launch $ mkdir -p config
Create a new bus configuration folder with the name
robot_control
.
$ mkdir -p config/robot_control
Create a new bus configuration file with the name
bus.yml
.
$ touch config/robot_control/bus.yml
Add the following content to the
bus.yml
file.
options:
dcf_path: "@BUS_CONFIG_PATH@"
master:
node_id: 1
driver: "ros2_canopen::MasterDriver"
package: "canopen_master_driver"
sync_period: 10000
defaults:
dcf: "cia402_slave.eds"
driver: "ros2_canopen::Cia402Driver"
package: "canopen_402_driver"
period: 10
position_mode: 1
revision_number: 0
sdo:
- {index: 0x60C2, sub_index: 1, value: 50} # Set interpolation time for cyclic modes to 50 ms
- {index: 0x60C2, sub_index: 2, value: -3} # Set base 10-3s
- {index: 0x6081, sub_index: 0, value: 1000}
- {index: 0x6083, sub_index: 0, value: 2000}
- {index: 0x6060, sub_index: 0, value: 7}
tpdo: # TPDO needed statusword, actual velocity, actual position, mode of operation
1:
enabled: true
cob_id: "auto"
transmission: 0x01
mapping:
- {index: 0x6041, sub_index: 0} # status word
- {index: 0x6061, sub_index: 0} # mode of operation display
2:
enabled: true
cob_id: "auto"
transmission: 0x01
mapping:
- {index: 0x6064, sub_index: 0} # position actual value
- {index: 0x606c, sub_index: 0} # velocity actual position
rpdo: # RPDO needed controlword, target position, target velocity, mode of operation
1:
enabled: true
cob_id: "auto"
mapping:
- {index: 0x6040, sub_index: 0} # controlword
- {index: 0x6060, sub_index: 0} # mode of operation
2:
enabled: true
cob_id: "auto"
mapping:
- {index: 0x607A, sub_index: 0} # target position
nodes:
joint_1:
node_id: 2
joint_2:
node_id: 3
Copy the
cia402_slave.eds
file from thecanopen_tests/config/robot_control
package to theconfig/robot_control
folder.Create a ros2_controllers.yaml and add the following content.
controller_manager:
ros__parameters:
update_rate: 100 # Hz
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
forward_position_controller:
type: forward_command_controller/ForwardCommandController
forward_position_controller:
ros__parameters:
joints:
- joint1
- joint2
interface_name: position
Create a launch file with the name
robot_control.launch.py
in the launch directory of your package and add the following content.
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
def generate_launch_description():
robot_description_content = Command(
[
PathJoinSubstitution([FindExecutable(name="xacro")]),
" ",
PathJoinSubstitution(
[
FindPackageShare("canopen_tests"),
"urdf",
"robot_controller",
"robot_controller.urdf.xacro",
]
),
]
)
robot_description = {"robot_description": robot_description_content}
robot_control_config = PathJoinSubstitution(
[FindPackageShare("canopen_tests"), "config/robot_control", "ros2_controllers.yaml"]
)
control_node = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[robot_description, robot_control_config],
output="screen",
)
joint_state_broadcaster_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)
forward_position_controller_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["forward_position_controller", "--controller-manager", "/controller_manager"],
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
output="both",
parameters=[robot_description],
)
slave_config = PathJoinSubstitution(
[FindPackageShare("canopen_tests"), "config/robot_control", "cia402_slave.eds"]
)
slave_launch = PathJoinSubstitution(
[FindPackageShare("canopen_fake_slaves"), "launch", "cia402_slave.launch.py"]
)
slave_node_1 = IncludeLaunchDescription(
PythonLaunchDescriptionSource(slave_launch),
launch_arguments={
"node_id": "2",
"node_name": "slave_node_1",
"slave_config": slave_config,
}.items(),
)
slave_node_2 = IncludeLaunchDescription(
PythonLaunchDescriptionSource(slave_launch),
launch_arguments={
"node_id": "3",
"node_name": "slave_node_2",
"slave_config": slave_config,
}.items(),
)
nodes_to_start = [
control_node,
joint_state_broadcaster_spawner,
forward_position_controller_spawner,
robot_state_publisher_node,
slave_node_1,
slave_node_2
]
return LaunchDescription(nodes_to_start)
Create a urdf folder add all files from the
canopen_tests/urdf/robot_controller
package to the urdf folder of your package.Edit the CMakeLists.txt file of your package and add the following lines after the find_package section.
cogen_dcf(robot_control)
install(DIRECTORY
launch urdf
DESTINATION share/${PROJECT_NAME})
Build your package and source the setup.bash file.
Start your launch file
You can now control the robot with the forward_command_controller. You can as well visualize the robot in rviz by adding a tf or a robot model and setting the fixed frame to
base_link
. You can move the robot with the following command.
ros2 topic pub /joint1/forward_position_controller/command std_msgs/msg/Float64 "data: [1.0, 1.0]"