Unverified Commit ecb96f53 authored by Karsten Knese's avatar Karsten Knese Committed by GitHub
Browse files

Gazebo support (#85)



* [ros2_control_demo_robot] Add gazebo for rrbot position_only demo.
* [ros2_control_demo_robot] Add missing launch file and controller position file.
* [ros2_control_demo_robot] Makes flake8 happy for rrbot_system_position_only_gazebo.launch.py
* [ros2_control_demo_robot] Fix year in Copyright notice of gazebo launch file.
* [package.xml] Adding xacro dependency
* [ros2_control_demo_hardware] Fix entity name when launching gazebo for rrbot.
* [ros2_control_demo_robot] Change xacro parameter simu to use_simu
* use generic dependency
* organize urdf files to work with gazebo
* start gazebo non-verbose
* correctly format urdf xacro
* use spawner script to load controller
* fix python imports, run precommit
Co-authored-by: Olivier Stasse's avatarOlivier Stasse <ostasse@laas.fr>
Co-authored-by: default avatarBence Magyar <bence.magyar.robotics@gmail.com>
parent 4cb48989
......@@ -23,5 +23,7 @@ if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
endif()
ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in")
## EXPORTS
ament_package()
controller_manager:
ros__parameters:
update_rate: 100 # Hz
joint_trajectory_controller:
type: joint_trajectory_controller/JointTrajectoryController
joint_state_broadcaster:
type: joint_state_broadcaster/JointStateBroadcaster
joint_trajectory_controller:
ros__parameters:
joints:
- joint1
- joint2
interface_name: position
......@@ -9,9 +9,8 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
<!-- ros_control plugin -->
<gazebo>
<plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
<robotNamespace>/rrbot</robotNamespace>
<robotSimType>gazebo_ros_control/DefaultRobotHWSim</robotSimType>
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
<parameters>$(find ros2_control_demo_robot)/config/rrbot_gazebo_forward_controller_position.yaml</parameters>
</plugin>
</gazebo>
......@@ -71,7 +70,7 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
<stddev>0.01</stddev>
</noise>
</ray>
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_gpu_laser.so">
<plugin name="gazebo_ros_head_hokuyo_controller" filename="libgazebo_ros_ray_sensor.so">
<topicName>/rrbot/laser/scan</topicName>
<frameName>hokuyo_link</frameName>
</plugin>
......
This diff is collapsed.
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="rrbot_system_position_only" params="name prefix slowdown:=2.0">
<xacro:macro name="rrbot_system_position_only" params="name prefix use_sim:='false' slowdown:=2.0">
<ros2_control name="${name}" type="system">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
<param name="example_param_hw_start_duration_sec">2.0</param>
<param name="example_param_hw_stop_duration_sec">3.0</param>
<param name="example_param_hw_slowdown">${slowdown}</param>
</hardware>
<xacro:if value="$(arg use_sim)">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
</xacro:if>
<xacro:unless value="$(arg use_sim)">
<hardware>
<plugin>ros2_control_demo_hardware/RRBotSystemPositionOnlyHardware</plugin>
<param name="example_param_hw_start_duration_sec">2.0</param>
<param name="example_param_hw_stop_duration_sec">3.0</param>
<param name="example_param_hw_slowdown">${slowdown}</param>
</hardware>
</xacro:unless>
<joint name="joint1">
<command_interface name="position">
<param name="min">-1</param>
......
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="rrbot" params="parent prefix *origin">
<!-- Constants for robot dimensions -->
<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="height1" value="0.2" /> <!-- Link 1 -->
<xacro:property name="height2" value="1" /> <!-- Link 2 -->
<xacro:property name="height3" value="1" /> <!-- Link 3 -->
<xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
<joint name="${prefix}base_joint" type="fixed">
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
<child link="${prefix}base_link" />
</joint>
<!-- Base Link -->
<link name="${prefix}base_link">
<collision>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height1}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height1}"/>
</geometry>
<material name="orange"/>
</visual>
<inertial>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${mass / 12.0 * (width*width + height1*height1)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
</link>
<joint name="${prefix}joint1" type="continuous">
<parent link="${prefix}base_link"/>
<child link="${prefix}link1"/>
<origin xyz="0 ${width} ${height1 - axel_offset}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<!-- Middle Link -->
<link name="${prefix}link1">
<collision>
<origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height2}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height2}"/>
</geometry>
<material name="yellow"/>
</visual>
<inertial>
<origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${mass / 12.0 * (width*width + height2*height2)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height2*height2 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
</link>
<joint name="${prefix}joint2" type="continuous">
<parent link="${prefix}link1"/>
<child link="${prefix}link2"/>
<origin xyz="0 ${width} ${height2 - axel_offset*2}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<!-- Top Link -->
<link name="${prefix}link2">
<collision>
<origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height3}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height3}"/>
</geometry>
<material name="orange"/>
</visual>
<inertial>
<origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${mass / 12.0 * (width*width + height3*height3)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height3*height3 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
</link>
<joint name="${prefix}tool_joint" type="fixed">
<origin xyz="0 0 1" rpy="0 0 0" />
<parent link="${prefix}link2"/>
<child link="${prefix}tool_link" />
</joint>
<!-- Tool Link -->
<link name="${prefix}tool_link">
</link>
<joint name="${prefix}hokuyo_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="0 0 ${height3 - axel_offset/2}" rpy="0 0 0"/>
<parent link="${prefix}link2"/>
<child link="${prefix}hokuyo_link"/>
</joint>
<!-- Hokuyo Laser -->
<link name="${prefix}hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://rrbot_description/meshes/hokuyo.dae"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<joint name="${prefix}camera_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="${camera_link} 0 ${height3 - axel_offset*2}" rpy="0 0 0"/>
<parent link="${prefix}link2"/>
<child link="${prefix}camera_link"/>
</joint>
<!-- Camera -->
<link name="${prefix}camera_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${camera_link} ${camera_link} ${camera_link}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${camera_link} ${camera_link} ${camera_link}"/>
</geometry>
<material name="red"/>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<!-- generate an optical frame http://www.ros.org/reps/rep-0103.html#suffix-frames
so that ros and opencv can operate on the camera frame correctly -->
<joint name="${prefix}camera_optical_joint" type="fixed">
<!-- these values have to be these values otherwise the gazebo camera image
won't be aligned properly with the frame it is supposedly originating from -->
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
<parent link="${prefix}camera_link"/>
<child link="${prefix}camera_link_optical"/>
</joint>
<link name="${prefix}camera_link_optical">
</link>
</xacro:macro>
</robot>
......@@ -5,21 +5,22 @@ Copied and modified from ROS1 example:
https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_description/urdf/rrbot.xacro
-->
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="2dof_robot">
<xacro:arg name="use_sim" default="false" />
<!-- Enable setting arguments from the launch file -->
<xacro:arg name="slowdown" default="2.0" />
<!-- Import RRBot macro -->
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.urdf.xacro" />
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/urdf/rrbot.urdf.xacro" />
<!-- Import all Gazebo-customization elements, including Gazebo colors -->
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.gazebo.xacro" />
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/gazebo/rrbot.gazebo.xacro" />
<!-- Import Rviz colors -->
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot.materials.xacro" />
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/gazebo/rrbot.materials.xacro" />
<!-- Import RRBot ros2_control description -->
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/rrbot_system_position_only.ros2_control.xacro" />
<xacro:include filename="$(find ros2_control_demo_robot)/description/rrbot/ros2_control/rrbot_system_position_only.ros2_control.xacro" />
<!-- Used for fixing robot -->
<link name="world"/>
......@@ -33,6 +34,5 @@ https://github.com/ros-simulation/gazebo_ros_demos/blob/kinetic-devel/rrbot_desc
<xacro:rrbot_gazebo prefix="" />
<xacro:rrbot_system_position_only name="RRBotSystemPositionOnly" prefix="" slowdown="$(arg slowdown)" />
<xacro:rrbot_system_position_only name="RRBotSystemPositionOnly" prefix="" use_sim="$(arg use_sim)" slowdown="$(arg slowdown)" />
</robot>
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:macro name="rrbot" params="parent prefix *origin">
<!-- Constants for robot dimensions -->
<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="height1" value="2" /> <!-- Link 1 -->
<xacro:property name="height2" value="1" /> <!-- Link 2 -->
<xacro:property name="height3" value="1" /> <!-- Link 3 -->
<xacro:property name="camera_link" value="0.05" /> <!-- Size of square 'camera' box -->
<xacro:property name="axel_offset" value="0.05" /> <!-- Space btw top of beam and the each joint -->
<joint name="${prefix}base_joint" type="fixed">
<xacro:insert_block name="origin" />
<parent link="${parent}"/>
<child link="${prefix}base_link" />
</joint>
<!-- Base Link -->
<link name="${prefix}base_link">
<collision>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height1}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height1}"/>
</geometry>
<material name="orange"/>
</visual>
<inertial>
<origin xyz="0 0 ${height1/2}" rpy="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${mass / 12.0 * (width*width + height1*height1)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height1*height1 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
</link>
<joint name="${prefix}joint1" type="continuous">
<parent link="${prefix}base_link"/>
<child link="${prefix}link1"/>
<origin xyz="0 ${width} ${height1 - axel_offset}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<!-- Middle Link -->
<link name="${prefix}link1">
<collision>
<origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height2}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height2}"/>
</geometry>
<material name="yellow"/>
</visual>
<inertial>
<origin xyz="0 0 ${height2/2 - axel_offset}" rpy="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${mass / 12.0 * (width*width + height2*height2)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height2*height2 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
</link>
<joint name="${prefix}joint2" type="continuous">
<parent link="${prefix}link1"/>
<child link="${prefix}link2"/>
<origin xyz="0 ${width} ${height2 - axel_offset*2}" rpy="0 0 0"/>
<axis xyz="0 1 0"/>
<dynamics damping="0.7"/>
</joint>
<!-- Top Link -->
<link name="${prefix}link2">
<collision>
<origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height3}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
<geometry>
<box size="${width} ${width} ${height3}"/>
</geometry>
<material name="orange"/>
</visual>
<inertial>
<origin xyz="0 0 ${height3/2 - axel_offset}" rpy="0 0 0"/>
<mass value="${mass}"/>
<inertia
ixx="${mass / 12.0 * (width*width + height3*height3)}" ixy="0.0" ixz="0.0"
iyy="${mass / 12.0 * (height3*height3 + width*width)}" iyz="0.0"
izz="${mass / 12.0 * (width*width + width*width)}"/>
</inertial>
</link>
<joint name="${prefix}tool_joint" type="fixed">
<origin xyz="0 0 1" rpy="0 0 0" />
<parent link="${prefix}link2"/>
<child link="${prefix}tool_link" />
</joint>
<!-- Tool Link -->
<link name="${prefix}tool_link">
</link>
<joint name="${prefix}hokuyo_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="0 0 ${height3 - axel_offset/2}" rpy="0 0 0"/>
<parent link="${prefix}link2"/>
<child link="${prefix}hokuyo_link"/>
</joint>
<!-- Hokuyo Laser -->
<link name="${prefix}hokuyo_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="0.1 0.1 0.1"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<mesh filename="package://ros2_control_demo_robot/description/rrbot/meshes/hokuyo.dae"/>
</geometry>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<joint name="${prefix}camera_joint" type="fixed">
<axis xyz="0 1 0" />
<origin xyz="${camera_link} 0 ${height3 - axel_offset*2}" rpy="0 0 0"/>
<parent link="${prefix}link2"/>
<child link="${prefix}camera_link"/>
</joint>
<!-- Camera -->
<link name="${prefix}camera_link">
<collision>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${camera_link} ${camera_link} ${camera_link}"/>
</geometry>
</collision>
<visual>
<origin xyz="0 0 0" rpy="0 0 0"/>
<geometry>
<box size="${camera_link} ${camera_link} ${camera_link}"/>
</geometry>
<material name="red"/>
</visual>
<inertial>
<mass value="1e-5" />
<origin xyz="0 0 0" rpy="0 0 0"/>
<inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6" />
</inertial>
</link>
<!-- generate an optical frame http://www.ros.org/reps/rep-0103.html#suffix-frames
so that ros and opencv can operate on the camera frame correctly -->
<joint name="${prefix}camera_optical_joint" type="fixed">
<!-- these values have to be these values otherwise the gazebo camera image
won't be aligned properly with the frame it is supposedly originating from -->
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
<parent link="${prefix}camera_link"/>
<child link="${prefix}camera_link_optical"/>
</joint>
<link name="${prefix}camera_link_optical">
</link>
</xacro:macro>
</robot>
prepend-non-duplicate;GAZEBO_MODEL_PATH;share
......@@ -28,6 +28,7 @@ def generate_launch_description():
robot_description_path = os.path.join(
get_package_share_directory("ros2_control_demo_robot"),
"description",
"rrbot",
"rrbot_system_position_only.urdf.xacro",
)
robot_description_config = xacro.process_file(
......
# Copyright 2021 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
import xacro
def generate_launch_description():
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource(
[
os.path.join(get_package_share_directory("gazebo_ros"), "launch"),
"/gazebo.launch.py",
]
),
launch_arguments={"verbose": "false"}.items(),
)
robot_description_path = os.path.join(
get_package_share_directory("ros2_control_demo_robot"),
"description",
"rrbot",
"rrbot_system_position_only.urdf.xacro",
)