changed for using name cps_rmp220_support package

This commit is contained in:
Björn Ellensohn 2023-04-12 10:11:46 +02:00
parent a500db5b14
commit 8ccc4acf08
26 changed files with 2193 additions and 14 deletions

22
.vscode/c_cpp_properties.json vendored Normal file
View File

@ -0,0 +1,22 @@
{
"configurations": [
{
"browse": {
"databaseFilename": "${default}",
"limitSymbolsToIncludedHeaders": false
},
"includePath": [
"/home/bjorn/Documents/ros-projects/cps_rmp220_ws/install/segway_msgs/include/**",
"/opt/ros/humble/include/**",
"/home/bjorn/Documents/ros-projects/cps_rmp220_ws/src/segwayrmp/include/**",
"/usr/include/**"
],
"name": "ROS",
"intelliSenseMode": "gcc-x64",
"compilerPath": "/usr/bin/gcc",
"cStandard": "gnu11",
"cppStandard": "c++14"
}
],
"version": 4
}

12
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,12 @@
{
"python.autoComplete.extraPaths": [
"/home/bjorn/Documents/ros-projects/cps_rmp220_ws/install/segway_msgs/local/lib/python3.10/dist-packages",
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
],
"python.analysis.extraPaths": [
"/home/bjorn/Documents/ros-projects/cps_rmp220_ws/install/segway_msgs/local/lib/python3.10/dist-packages",
"/opt/ros/humble/lib/python3.10/site-packages",
"/opt/ros/humble/local/lib/python3.10/dist-packages"
]
}

View File

@ -1,5 +1,5 @@
cmake_minimum_required(VERSION 3.5) cmake_minimum_required(VERSION 3.5)
project(my_bot) project(cps_rmp220_support)
# Default to C99 # Default to C99
if(NOT CMAKE_C_STANDARD) if(NOT CMAKE_C_STANDARD)

View File

@ -1,7 +1,12 @@
## Robot Package Template ## Supplementary Robot PAckage for Segway RMP220 lite
This is a GitHub template. You can make your own copy by clicking the green "Use this template" button. This package should be placed into your workspace src/ folder.
Alongside you must clone https://github.com/bjoernellens1/segwayrmp and https://github.com/bjoernellens1/segway_msgs to control the robot.
Go back to ws folder and colcon build --symlink-install && source install/setup.bash
It is recommended that you keep the repo/package name the same, but if you do change it, ensure you do a "Find all" using your IDE (or the built-in GitHub IDE by hitting the `.` key) and rename all instances of `my_bot` to whatever your project's name is. ### Launch files:
#### Launch Gazebo
ros2 launch cps_rmp220_support launch_sim.launch.py
Note that each directory currently has at least one file in it to ensure that git tracks the files (and, consequently, that a fresh clone has direcctories present for CMake to find). These example files can be removed if required (and the directories can be removed if `CMakeLists.txt` is adjusted accordingly). #### Launch Robot operator (Only neeed for Camera and additional devices for now)
ros2 launch cps_rmp220_support launch_robot.launch.py

View File

@ -0,0 +1,3 @@
controller_manager:
ros__parameters:
use_sim_time: true

27
config/joystick.yaml Normal file
View File

@ -0,0 +1,27 @@
joy_node:
ros__parameters:
device_id: 0
deadzone: 0.05
autorepeat_rate: 20.0
teleop_node:
ros__parameters:
axis_linear:
x: 1
scale_linear:
x: 0.7
scale_linear_turbo:
x: 1.5
axis_angular:
yaw: 0
scale_angular:
yaw: 0.5
scale_angular_turbo:
yaw: 1.0
enable_button: 2
enable_turbo_button: 5
require_enable_button: true

View File

@ -0,0 +1,68 @@
controller_manager:
ros__parameters:
update_rate: 30
use_sim_time: false
diff_cont:
type: diff_drive_controller/DiffDriveController
joint_broad:
type: joint_state_broadcaster/JointStateBroadcaster
diff_cont:
ros__parameters:
publish_rate: 50.0
base_frame_id: base_link
left_wheel_names: ['left_wheel_joint']
right_wheel_names: ['right_wheel_joint']
wheel_separation: 0.35
wheel_radius: 0.05
use_stamped_vel: false
# open_loop: false
# wheels_per_side: x
# wheel_separation_multiplier: x
# left_wheel_radius_multiplier: x
# right_wheel_radius_multiplier: x
# odom_frame_id: x
# pose_covariance_diagonal: x
# twist_covariance_diagonal: x
# open_loop: x
# enable_odom_tf: x
# cmd_vel_timeout: x
# publish_limited_velocity: x
# velocity_rolling_window_size: x
# linear.x.has_velocity_limits: false
# linear.x.has_acceleration_limits: false
# linear.x.has_jerk_limits: false
# linear.x.max_velocity: NAN
# linear.x.min_velocity: NAN
# linear.x.max_acceleration: NAN
# linear.x.min_acceleration: NAN
# linear.x.max_jerk: NAN
# linear.x.min_jerk: NAN
# angular.z.has_velocity_limits: false
# angular.z.has_acceleration_limits: false
# angular.z.has_jerk_limits: false
# angular.z.max_velocity: NAN
# angular.z.min_velocity: NAN
# angular.z.max_acceleration: NAN
# angular.z.min_acceleration: NAN
# angular.z.max_jerk: NAN
# angular.z.min_jerk: NAN
# joint_broad:
# ros__parameters:

15
config/twist_mux.yaml Normal file
View File

@ -0,0 +1,15 @@
twist_mux:
ros__parameters:
topics:
navigation:
topic : cmd_vel
timeout : 0.5
priority: 10
tracker:
topic : cmd_vel_tracker
timeout : 0.5
priority: 20
joystick:
topic : cmd_vel_joy
timeout : 0.5
priority: 100

204
config/view_bot.rviz Normal file
View File

@ -0,0 +1,204 @@
Panels:
- Class: rviz_common/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /TF1
- /RobotModel1
- /RobotModel1/Status1
Splitter Ratio: 0.5
Tree Height: 759
- Class: rviz_common/Selection
Name: Selection
- Class: rviz_common/Tool Properties
Expanded:
- /2D Goal Pose1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz_common/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz_default_plugins/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz_default_plugins/TF
Enabled: true
Frame Timeout: 15
Frames:
All Enabled: true
base_link:
Value: true
caster_wheel:
Value: true
chassis:
Value: true
left_wheel:
Value: true
right_wheel:
Value: true
Marker Scale: 1
Name: TF
Show Arrows: true
Show Axes: true
Show Names: true
Tree:
base_link:
chassis:
caster_wheel:
{}
left_wheel:
{}
right_wheel:
{}
Update Interval: 0
Value: true
- Alpha: 1
Class: rviz_default_plugins/RobotModel
Collision Enabled: false
Description File: ""
Description Source: Topic
Description Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /robot_description
Enabled: true
Links:
All Links Enabled: true
Expand Joint Details: false
Expand Link Details: false
Expand Tree: false
Link Tree Style: Links in Alphabetic Order
base_link:
Alpha: 1
Show Axes: false
Show Trail: false
caster_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
chassis:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
left_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
right_wheel:
Alpha: 1
Show Axes: false
Show Trail: false
Value: true
Name: RobotModel
TF Prefix: ""
Update Interval: 0
Value: true
Visual Enabled: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Fixed Frame: base_link
Frame Rate: 30
Name: root
Tools:
- Class: rviz_default_plugins/Interact
Hide Inactive Objects: true
- Class: rviz_default_plugins/MoveCamera
- Class: rviz_default_plugins/Select
- Class: rviz_default_plugins/FocusCamera
- Class: rviz_default_plugins/Measure
Line color: 128; 128; 0
- Class: rviz_default_plugins/SetInitialPose
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /initialpose
- Class: rviz_default_plugins/SetGoal
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /goal_pose
- Class: rviz_default_plugins/PublishPoint
Single click: true
Topic:
Depth: 5
Durability Policy: Volatile
History Policy: Keep Last
Reliability Policy: Reliable
Value: /clicked_point
Transformation:
Current:
Class: rviz_default_plugins/TF
Value: true
Views:
Current:
Class: rviz_default_plugins/Orbit
Distance: 1.0897012948989868
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 0.4002038538455963
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 5.888577461242676
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 997
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd00000004000000000000016a00000386fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000004100000386000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000386fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000004100000386000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d00650100000000000004500000000000000000000004fb0000038600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1920
X: 0
Y: 28

62
description/camera.xacro Normal file
View File

@ -0,0 +1,62 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<joint name="camera_joint" type="fixed">
<parent link="chassis"/>
<child link="camera_link"/>
<origin xyz="0.276 0 0.181" rpy="0 0.18 0"/>
</joint>
<link name="camera_link">
<visual>
<geometry>
<box size="0.010 0.03 0.03"/>
</geometry>
<material name="black"/>
</visual>
<visual>
<origin xyz="0 0 -0.05"/>
<geometry>
<cylinder radius="0.002" length="0.1"/>
</geometry>
<material name="black"/>
</visual>
</link>
<joint name="camera_optical_joint" type="fixed">
<parent link="camera_link"/>
<child link="camera_link_optical"/>
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
</joint>
<link name="camera_link_optical"></link>
<gazebo reference="camera_link">
<material>Gazebo/Black</material>
<sensor name="camera" type="camera">
<pose> 0 0 0 0 0 0 </pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<camera>
<horizontal_fov>1.089</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<frame_name>camera_link_optical</frame_name>
</plugin>
</sensor>
</gazebo>
</robot>

View File

@ -0,0 +1,57 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<joint name="camera_joint" type="fixed">
<parent link="chassis"/>
<child link="camera_link"/>
<origin xyz="0.305 0 0.08" rpy="0 0 0"/>
</joint>
<link name="camera_link">
<visual>
<geometry>
<box size="0.010 0.03 0.03"/>
</geometry>
<material name="red"/>
</visual>
</link>
<joint name="camera_optical_joint" type="fixed">
<parent link="camera_link"/>
<child link="camera_link_optical"/>
<origin xyz="0 0 0" rpy="${-pi/2} 0 ${-pi/2}"/>
</joint>
<link name="camera_link_optical"></link>
<gazebo reference="camera_link">
<material>Gazebo/Red</material>
<sensor name="camera" type="depth">
<pose> 0 0 0 0 0 0 </pose>
<visualize>true</visualize>
<update_rate>10</update_rate>
<camera>
<horizontal_fov>1.089</horizontal_fov>
<image>
<format>B8G8R8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.05</near>
<far>8.0</far>
</clip>
</camera>
<plugin name="camera_controller" filename="libgazebo_ros_camera.so">
<frame_name>camera_link_optical</frame_name>
<min_depth>0.1</min_depth>
<max_depth>100.0</max_depth>
</plugin>
</sensor>
</gazebo>
</robot>

42
description/face.xacro Executable file
View File

@ -0,0 +1,42 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<joint name="face_joint" type="fixed">
<parent link="chassis"/>
<child link="face_link"/>
<origin xyz="${chassis_length} 0 ${chassis_height/2}" rpy="0 0 0"/>
</joint>
<link name="face_link">
<visual>
<origin xyz="0 0.05 0.01" rpy="0 ${pi/2} 0"/>
<geometry>
<cylinder radius="0.01" length="0.002"/>
</geometry>
<material name="black"/>
</visual>
<visual>
<origin xyz="0 -0.05 0.01" rpy="0 ${pi/2} 0"/>
<geometry>
<cylinder radius="0.01" length="0.002"/>
</geometry>
<material name="black"/>
</visual>
<visual>
<origin xyz="-0.011 0 -0.00" rpy="0 ${1.5} 0"/>
<geometry>
<cylinder radius="0.05" length="0.02"/>
</geometry>
<material name="red"/>
</visual>
</link>
<gazebo reference="face_link">
<material>Gazebo/Black</material>
</gazebo>
</robot>

View File

@ -0,0 +1,31 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<gazebo>
<plugin name="diff_drive" filename="libgazebo_ros_diff_drive.so">
<!-- Wheel Information -->
<left_joint>left_wheel_joint</left_joint>
<right_joint>right_wheel_joint</right_joint>
<wheel_separation>0.297</wheel_separation>
<wheel_diameter>0.066</wheel_diameter>
<!-- Limits -->
<max_wheel_torque>200</max_wheel_torque>
<max_wheel_acceleration>10.0</max_wheel_acceleration>
<!-- Output -->
<odometry_frame>odom</odometry_frame>
<robot_base_frame>base_link</robot_base_frame>
<publish_odom>true</publish_odom>
<publish_odom_tf>true</publish_odom_tf>
<publish_wheel_tf>true</publish_wheel_tf>
</plugin>
</gazebo>
</robot>

View File

@ -0,0 +1,40 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<!-- Specify some standard inertial calculations https://en.wikipedia.org/wiki/List_of_moments_of_inertia -->
<!-- These make use of xacro's mathematical functionality -->
<xacro:macro name="inertial_sphere" params="mass radius *origin">
<inertial>
<xacro:insert_block name="origin"/>
<mass value="${mass}" />
<inertia ixx="${(2/5) * mass * (radius*radius)}" ixy="0.0" ixz="0.0"
iyy="${(2/5) * mass * (radius*radius)}" iyz="0.0"
izz="${(2/5) * mass * (radius*radius)}" />
</inertial>
</xacro:macro>
<xacro:macro name="inertial_box" params="mass x y z *origin">
<inertial>
<xacro:insert_block name="origin"/>
<mass value="${mass}" />
<inertia ixx="${(1/12) * mass * (y*y+z*z)}" ixy="0.0" ixz="0.0"
iyy="${(1/12) * mass * (x*x+z*z)}" iyz="0.0"
izz="${(1/12) * mass * (x*x+y*y)}" />
</inertial>
</xacro:macro>
<xacro:macro name="inertial_cylinder" params="mass length radius *origin">
<inertial>
<xacro:insert_block name="origin"/>
<mass value="${mass}" />
<inertia ixx="${(1/12) * mass * (3*radius*radius + length*length)}" ixy="0.0" ixz="0.0"
iyy="${(1/12) * mass * (3*radius*radius + length*length)}" iyz="0.0"
izz="${(1/2) * mass * (radius*radius)}" />
</inertial>
</xacro:macro>
</robot>

66
description/lidar.xacro Normal file
View File

@ -0,0 +1,66 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<joint name="laser_joint" type="fixed">
<parent link="chassis"/>
<child link="laser_frame"/>
<origin xyz="0.122 0 0.212" rpy="0 0 0"/>
</joint>
<link name="laser_frame">
<visual>
<geometry>
<cylinder radius="0.05" length="0.04"/>
</geometry>
<material name="black"/>
</visual>
<visual>
<origin xyz="0 0 -0.05"/>
<geometry>
<cylinder radius="0.01" length="0.1"/>
</geometry>
<material name="black"/>
</visual>
<collision>
<geometry>
<cylinder radius="0.05" length="0.04"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="0.1" length="0.04" radius="0.05">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<gazebo reference="laser_frame">
<material>Gazebo/Black</material>
<sensor name="laser" type="ray">
<pose> 0 0 0 0 0 0 </pose>
<visualize>false</visualize>
<update_rate>10</update_rate>
<ray>
<scan>
<horizontal>
<samples>360</samples>
<min_angle>-3.14</min_angle>
<max_angle>3.14</max_angle>
</horizontal>
</scan>
<range>
<min>0.3</min>
<max>12</max>
</range>
</ray>
<plugin name="laser_controller" filename="libgazebo_ros_ray_sensor.so">
<ros>
<argument>~/out:=scan</argument>
</ros>
<output_type>sensor_msgs/LaserScan</output_type>
<frame_name>laser_frame</frame_name>
</plugin>
</sensor>
</gazebo>
</robot>

View File

@ -1,7 +1,21 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot"> <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="robot">
<!-- Example link --> <xacro:arg name="use_ros2_control" default="true"/>
<link name="base_link"></link> <xacro:arg name="sim_mode" default="false"/>
<xacro:include filename="robot_core.xacro" />
<xacro:if value="$(arg use_ros2_control)">
<xacro:include filename="ros2_control.xacro" />
</xacro:if>
<xacro:unless value="$(arg use_ros2_control)">
<xacro:include filename="gazebo_control.xacro" />
</xacro:unless>
<xacro:include filename="lidar.xacro" />
<xacro:include filename="camera.xacro" />
<!-- <xacro:include filename="depth_camera.xacro" /> -->
<xacro:include filename="face.xacro" />
</robot> </robot>

View File

@ -0,0 +1,204 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
<xacro:include filename="inertial_macros.xacro"/>
<xacro:property name="chassis_length" value="0.600"/>
<xacro:property name="chassis_width" value="0.330"/>
<xacro:property name="chassis_height" value="0.150"/>
<xacro:property name="chassis_mass" value="15.0"/>
<xacro:property name="wheel_radius" value="0.135"/>
<xacro:property name="wheel_thickness" value="0.080"/>
<xacro:property name="wheel_mass" value="4.0"/>
<xacro:property name="wheel_offset_x" value="0.285748"/>
<xacro:property name="wheel_offset_y" value="0.33675"/>
<xacro:property name="wheel_offset_z" value="0.01"/>
<xacro:property name="caster_wheel_radius" value="0.01"/>
<xacro:property name="caster_wheel_mass" value="0.01"/>
<xacro:property name="caster_wheel_offset_x" value="0.075"/>
<xacro:property name="caster_wheel_offset_z" value="${wheel_offset_z - wheel_radius + caster_wheel_radius}"/>
<material name="white">
<color rgba="1 1 1 1" />
</material>
<material name="orange">
<color rgba="1 0.3 0.1 1"/>
</material>
<material name="blue">
<color rgba="0.2 0.2 1 1"/>
</material>
<material name="black">
<color rgba="0 0 0 1"/>
</material>
<material name="red">
<color rgba="1 0 0 1"/>
</material>
<material name="green">
<color rgba="0.0 0.8 0.0 1.0"/>
</material>
<material name="grey">
<color rgba="0.6 0.6 0.6 1.0"/>
</material>
<material name="grey2">
<color rgba="0.9 0.9 0.9 1.0"/>
</material>
<material name="lightGrey">
<color rgba="0.6 0.6 0.6 1.0"/>
</material>
<!-- BASE LINK -->
<link name="base_link">
</link>
<!-- BASE_FOOTPRINT LINK -->
<joint name="base_footprint_joint" type="fixed">
<parent link="base_link"/>
<child link="base_footprint"/>
<origin xyz="0 0 0" rpy="0 0 0"/>
</joint>
<link name="base_footprint">
</link>
<!-- CHASSIS LINK -->
<joint name="chassis_joint" type="fixed">
<parent link="base_link"/>
<child link="chassis"/>
<origin xyz="${-wheel_offset_x} 0 ${-wheel_offset_z}"/>
</joint>
<link name="chassis">
<visual>
<origin xyz="${chassis_length/2} 0 ${chassis_height/2}"/>
<geometry>
<box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
</geometry>
<material name="orange"/>
</visual>
<collision>
<origin xyz="${chassis_length/2} 0 ${chassis_height/2}"/>
<geometry>
<box size="${chassis_length} ${chassis_width} ${chassis_height}"/>
</geometry>
</collision>
<xacro:inertial_box mass="0.5" x="${chassis_length}" y="${chassis_width}" z="${chassis_height}">
<origin xyz="${chassis_length/2} 0 ${chassis_height/2}" rpy="0 0 0"/>
</xacro:inertial_box>
</link>
<gazebo reference="chassis">
<material>Gazebo/Orange</material>
</gazebo>
<!-- LEFT WHEEL LINK -->
<joint name="left_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="left_wheel"/>
<origin xyz="0 ${wheel_offset_y} 0" rpy="-${pi/2} 0 0" />
<axis xyz="0 0 1"/>
</joint>
<link name="left_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<sphere radius="${wheel_radius}"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_thickness}" radius="${wheel_radius}">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<gazebo reference="left_wheel">
<material>Gazebo/Blue</material>
</gazebo>
<!-- RIGHT WHEEL LINK -->
<joint name="right_wheel_joint" type="continuous">
<parent link="base_link"/>
<child link="right_wheel"/>
<origin xyz="0 ${-wheel_offset_y} 0" rpy="${pi/2} 0 0" />
<axis xyz="0 0 -1"/>
</joint>
<link name="right_wheel">
<visual>
<geometry>
<cylinder radius="${wheel_radius}" length="${wheel_thickness}"/>
</geometry>
<material name="blue"/>
</visual>
<collision>
<geometry>
<sphere radius="${wheel_radius}"/>
</geometry>
</collision>
<xacro:inertial_cylinder mass="${wheel_mass}" length="${wheel_thickness}" radius="${wheel_radius}">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_cylinder>
</link>
<gazebo reference="right_wheel">
<material>Gazebo/Blue</material>
</gazebo>
<!-- CASTER WHEEL LINK -->
<joint name="caster_wheel_joint" type="fixed">
<parent link="chassis"/>
<child link="caster_wheel"/>
<origin xyz="${caster_wheel_offset_x} 0 ${caster_wheel_offset_z}"/>
</joint>
<link name="caster_wheel">
<visual>
<geometry>
<sphere radius="${caster_wheel_radius}"/>
</geometry>
<material name="white"/>
</visual>
<collision>
<geometry>
<sphere radius="${caster_wheel_radius}"/>
</geometry>
</collision>
<xacro:inertial_sphere mass="${caster_wheel_mass}" radius="${caster_wheel_radius}">
<origin xyz="0 0 0" rpy="0 0 0"/>
</xacro:inertial_sphere>
</link>
<gazebo reference="caster_wheel">
<material>Gazebo/White</material>
<mu1 value="0.001"/>
<mu2 value="0.001"/>
</gazebo>
</robot>

View File

@ -0,0 +1,66 @@
<?xml version="1.0"?>
<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
<xacro:unless value="$(arg sim_mode)">
<ros2_control name="RealRobot" type="system">
<hardware>
<plugin>diffdrive_arduino/DiffDriveArduino</plugin>
<param name="left_wheel_name">left_wheel_joint</param>
<param name="right_wheel_name">right_wheel_joint</param>
<param name="loop_rate">30</param>
<param name="device">/dev/ttyUSB0</param>
<param name="baud_rate">57600</param>
<param name="timeout">1000</param>
<param name="enc_counts_per_rev">3436</param>
</hardware>
<joint name="left_wheel_joint">
<command_interface name="velocity">
<param name="min">-10</param>
<param name="max">10</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/>
</joint>
<joint name="right_wheel_joint">
<command_interface name="velocity">
<param name="min">-10</param>
<param name="max">10</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/>
</joint>
</ros2_control>
</xacro:unless>
<xacro:if value="$(arg sim_mode)">
<ros2_control name="GazeboSystem" type="system">
<hardware>
<plugin>gazebo_ros2_control/GazeboSystem</plugin>
</hardware>
<joint name="left_wheel_joint">
<command_interface name="velocity">
<param name="min">-10</param>
<param name="max">10</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/>
</joint>
<joint name="right_wheel_joint">
<command_interface name="velocity">
<param name="min">-10</param>
<param name="max">10</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/>
</joint>
</ros2_control>
</xacro:if>
<gazebo>
<plugin name="gazebo_ros2_control" filename="libgazebo_ros2_control.so">
<parameters>$(find segway_rmp220_support)/config/my_controllers.yaml</parameters>
<parameters>$(find segway_rmp220_support)/config/gaz_ros2_ctl_use_sim.yaml</parameters>
</plugin>
</gazebo>
</robot>

23
launch/camera.launch.py Normal file
View File

@ -0,0 +1,23 @@
import os
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='v4l2_camera',
executable='v4l2_camera_node',
output='screen',
namespace='camera',
parameters=[{
'image_size': [640,480],
'time_per_frame': [1, 6],
'camera_frame_id': 'camera_link_optical'
}]
)
])

45
launch/joystick.launch.py Normal file
View File

@ -0,0 +1,45 @@
from launch import LaunchDescription
from launch_ros.actions import Node
from launch.substitutions import LaunchConfiguration
from launch.actions import DeclareLaunchArgument
import os
from ament_index_python.packages import get_package_share_directory
def generate_launch_description():
use_sim_time = LaunchConfiguration('use_sim_time')
joy_params = os.path.join(get_package_share_directory('segway_rmp220_support'),'config','joystick.yaml')
joy_node = Node(
package='joy',
executable='joy_node',
parameters=[joy_params, {'use_sim_time': use_sim_time}],
)
teleop_node = Node(
package='teleop_twist_joy',
executable='teleop_node',
name='teleop_node',
parameters=[joy_params, {'use_sim_time': use_sim_time}],
remappings=[('/cmd_vel','/cmd_vel_joy')]
)
# twist_stamper = Node(
# package='twist_stamper',
# executable='twist_stamper',
# parameters=[{'use_sim_time': use_sim_time}],
# remappings=[('/cmd_vel_in','/diff_cont/cmd_vel_unstamped'),
# ('/cmd_vel_out','/diff_cont/cmd_vel')]
# )
return LaunchDescription([
DeclareLaunchArgument(
'use_sim_time',
default_value='false',
description='Use sim time if true'),
joy_node,
teleop_node,
# twist_stamper
])

115
launch/launch_robot.launch.py Executable file
View File

@ -0,0 +1,115 @@
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command
from launch.actions import RegisterEventHandler
from launch.event_handlers import OnProcessStart
from launch_ros.actions import Node
def generate_launch_description():
# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
# !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
package_name='segway_rmp220_support' #<--- CHANGE ME
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(package_name),'launch','rsp.launch.py'
)]), launch_arguments={'use_sim_time': 'false', 'use_ros2_control': 'true'}.items()
)
# joystick = IncludeLaunchDescription(
# PythonLaunchDescriptionSource([os.path.join(
# get_package_share_directory(package_name),'launch','joystick.launch.py'
# )])
# )
twist_mux_params = os.path.join(get_package_share_directory(package_name),'config','twist_mux.yaml')
twist_mux = Node(
package="twist_mux",
executable="twist_mux",
parameters=[twist_mux_params],
remappings=[('/cmd_vel_out','/diff_cont/cmd_vel_unstamped')]
)
robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description'])
controller_params_file = os.path.join(get_package_share_directory(package_name),'config','my_controllers.yaml')
controller_manager = Node(
package="controller_manager",
executable="ros2_control_node",
parameters=[{'robot_description': robot_description},
controller_params_file]
)
delayed_controller_manager = TimerAction(period=3.0, actions=[controller_manager])
diff_drive_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["diff_cont"],
)
delayed_diff_drive_spawner = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=controller_manager,
on_start=[diff_drive_spawner],
)
)
joint_broad_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_broad"],
)
delayed_joint_broad_spawner = RegisterEventHandler(
event_handler=OnProcessStart(
target_action=controller_manager,
on_start=[joint_broad_spawner],
)
)
# Code for delaying a node (I haven't tested how effective it is)
#
# First add the below lines to imports
# from launch.actions import RegisterEventHandler
# from launch.event_handlers import OnProcessExit
#
# Then add the following below the current diff_drive_spawner
# delayed_diff_drive_spawner = RegisterEventHandler(
# event_handler=OnProcessExit(
# target_action=spawn_entity,
# on_exit=[diff_drive_spawner],
# )
# )
#
# Replace the diff_drive_spawner in the final return with delayed_diff_drive_spawner
# Launch them all!
return LaunchDescription([
rsp,
# joystick,
twist_mux,
delayed_controller_manager,
delayed_diff_drive_spawner,
delayed_joint_broad_spawner
])

98
launch/launch_sim.launch.py Executable file
View File

@ -0,0 +1,98 @@
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
def generate_launch_description():
# Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
# !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
package_name='cps_rmp220_support' #<--- CHANGE ME
rsp = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(package_name),'launch','rsp.launch.py'
)]), launch_arguments={'use_sim_time': 'true', 'use_ros2_control': 'true'}.items()
)
joystick = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory(package_name),'launch','joystick.launch.py'
)]), launch_arguments={'use_sim_time': 'true'}.items()
)
twist_mux_params = os.path.join(get_package_share_directory(package_name),'config','twist_mux.yaml')
twist_mux = Node(
package="twist_mux",
executable="twist_mux",
parameters=[twist_mux_params, {'use_sim_time': True}],
remappings=[('/cmd_vel_out','/diff_cont/cmd_vel_unstamped')]
)
gazebo_params_file = os.path.join(get_package_share_directory(package_name),'config','gazebo_params.yaml')
# Include the Gazebo launch file, provided by the gazebo_ros package
gazebo = IncludeLaunchDescription(
PythonLaunchDescriptionSource([os.path.join(
get_package_share_directory('gazebo_ros'), 'launch', 'gazebo.launch.py')]),
launch_arguments={'extra_gazebo_args': '--ros-args --params-file ' + gazebo_params_file}.items()
)
# Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
spawn_entity = Node(package='gazebo_ros', executable='spawn_entity.py',
arguments=['-topic', 'robot_description',
'-entity', 'my_bot'],
output='screen')
diff_drive_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["diff_cont"],
)
joint_broad_spawner = Node(
package="controller_manager",
executable="spawner",
arguments=["joint_broad"],
)
# Code for delaying a node (I haven't tested how effective it is)
#
# First add the below lines to imports
# from launch.actions import RegisterEventHandler
# from launch.event_handlers import OnProcessExit
#
# Then add the following below the current diff_drive_spawner
# delayed_diff_drive_spawner = RegisterEventHandler(
# event_handler=OnProcessExit(
# target_action=spawn_entity,
# on_exit=[diff_drive_spawner],
# )
# )
#
# Replace the diff_drive_spawner in the final return with delayed_diff_drive_spawner
# Launch them all!
return LaunchDescription([
rsp,
joystick,
twist_mux,
gazebo,
spawn_entity,
diff_drive_spawner,
joint_broad_spawner
])

20
launch/rplidar.launch.py Normal file
View File

@ -0,0 +1,20 @@
import os
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='rplidar_ros',
executable='rplidar_composition',
output='screen',
parameters=[{
'serial_port': '/dev/serial/by-path/platform-fd500000.pcie-pci-0000:01:00.0-usb-0:1.3:1.0-port0',
'frame_id': 'laser_frame',
'angle_compensate': True,
'scan_mode': 'Standard'
}]
)
])

View File

@ -14,14 +14,15 @@ def generate_launch_description():
# Check if we're told to use sim time # Check if we're told to use sim time
use_sim_time = LaunchConfiguration('use_sim_time') use_sim_time = LaunchConfiguration('use_sim_time')
use_ros2_control = LaunchConfiguration('use_ros2_control')
# Process the URDF file # Process the URDF file
pkg_path = os.path.join(get_package_share_directory('my_bot')) pkg_path = os.path.join(get_package_share_directory('cps_rmp220_support'))
xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro') xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
robot_description_config = xacro.process_file(xacro_file) robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control, ' sim_mode:=', use_sim_time])
# Create a robot_state_publisher node # Create a robot_state_publisher node
params = {'robot_description': robot_description_config.toxml(), 'use_sim_time': use_sim_time} params = {'robot_description': robot_description_config, 'use_sim_time': use_sim_time}
node_robot_state_publisher = Node( node_robot_state_publisher = Node(
package='robot_state_publisher', package='robot_state_publisher',
executable='robot_state_publisher', executable='robot_state_publisher',
@ -36,6 +37,10 @@ def generate_launch_description():
'use_sim_time', 'use_sim_time',
default_value='false', default_value='false',
description='Use sim time if true'), description='Use sim time if true'),
DeclareLaunchArgument(
'use_ros2_control',
default_value='true',
description='Use ros2_control if true'),
node_robot_state_publisher node_robot_state_publisher
]) ])

View File

@ -1,10 +1,10 @@
<?xml version="1.0"?> <?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?> <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3"> <package format="3">
<name>my_bot</name> <name>cps_rmp220_support</name>
<version>0.0.0</version> <version>0.0.1</version>
<description>TODO: Package description</description> <description>Segway RMP220 lite ackage for control, simulation and visualization</description>
<maintainer email="my_email@email.com">MY NAME</maintainer> <maintainer email="bjoern.ellensohn@stud.unileoben.ac.at">MY NAME</maintainer>
<license>TODO: License declaration</license> <license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>

935
worlds/obstacles.world Normal file
View File

@ -0,0 +1,935 @@
<sdf version='1.7'>
<world name='default'>
<light name='sun' type='directional'>
<cast_shadows>1</cast_shadows>
<pose>0 0 10 0 -0 0</pose>
<diffuse>0.8 0.8 0.8 1</diffuse>
<specular>0.2 0.2 0.2 1</specular>
<attenuation>
<range>1000</range>
<constant>0.9</constant>
<linear>0.01</linear>
<quadratic>0.001</quadratic>
</attenuation>
<direction>-0.5 0.1 -0.9</direction>
<spot>
<inner_angle>0</inner_angle>
<outer_angle>0</outer_angle>
<falloff>0</falloff>
</spot>
</light>
<model name='ground_plane'>
<static>1</static>
<link name='link'>
<collision name='collision'>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<surface>
<contact>
<collide_bitmask>65535</collide_bitmask>
<ode/>
</contact>
<friction>
<ode>
<mu>100</mu>
<mu2>50</mu2>
</ode>
<torsional>
<ode/>
</torsional>
</friction>
<bounce/>
</surface>
<max_contacts>10</max_contacts>
</collision>
<visual name='visual'>
<cast_shadows>0</cast_shadows>
<geometry>
<plane>
<normal>0 0 1</normal>
<size>100 100</size>
</plane>
</geometry>
<material>
<script>
<uri>file://media/materials/scripts/gazebo.material</uri>
<name>Gazebo/Grey</name>
</script>
</material>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
</model>
<gravity>0 0 -9.8</gravity>
<magnetic_field>6e-06 2.3e-05 -4.2e-05</magnetic_field>
<atmosphere type='adiabatic'/>
<physics type='ode'>
<max_step_size>0.001</max_step_size>
<real_time_factor>1</real_time_factor>
<real_time_update_rate>1000</real_time_update_rate>
</physics>
<scene>
<ambient>0.4 0.4 0.4 1</ambient>
<background>0.7 0.7 0.7 1</background>
<shadows>1</shadows>
</scene>
<wind/>
<spherical_coordinates>
<surface_model>EARTH_WGS84</surface_model>
<latitude_deg>0</latitude_deg>
<longitude_deg>0</longitude_deg>
<elevation>0</elevation>
<heading_deg>0</heading_deg>
</spherical_coordinates>
<model name='Construction Barrel'>
<link name='link'>
<inertial>
<pose>0 0 0.4 0 -0 0</pose>
<mass>500</mass>
<inertia>
<ixx>51.2096</ixx>
<iyy>51.2096</iyy>
<izz>25</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://construction_barrel/meshes/construction_barrel.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://construction_barrel/meshes/construction_barrel.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-0.756237 1.39226 0 0 -0 0</pose>
</model>
<model name='Construction Barrel_0'>
<link name='link'>
<inertial>
<pose>0 0 0.4 0 -0 0</pose>
<mass>500</mass>
<inertia>
<ixx>51.2096</ixx>
<iyy>51.2096</iyy>
<izz>25</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://construction_barrel/meshes/construction_barrel.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://construction_barrel/meshes/construction_barrel.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>1.80022 0.377178 0 0 -0 0</pose>
</model>
<model name='Construction Cone'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-2.19745 -2.25046 0 0 -0 0</pose>
</model>
<model name='Construction Cone_0'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-1.40494 -2.55135 0 0 -0 0</pose>
</model>
<model name='Construction Cone_1'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-0.501986 -2.9745 0 0 -0 0</pose>
</model>
<model name='Construction Cone_2'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>2.09724 -2.18232 0 0 -0 0</pose>
</model>
<model name='Construction Cone_3'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-3.69879 0.345038 0 0 -0 0</pose>
</model>
<model name='Construction Cone_4'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-0.406438 -4.23374 0 0 -0 0</pose>
</model>
<model name='Construction Cone_5'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>2.38815 -3.2576 0 0 -0 0</pose>
</model>
<model name='Construction Cone_6'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>2.39316 -4.49605 0 0 -0 0</pose>
</model>
<model name='Construction Cone_7'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-3.00495 -2.78053 0 0 -0 0</pose>
</model>
<model name='Construction Cone_8'>
<link name='link'>
<collision name='collision'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<scale>10 10 10</scale>
<uri>model://construction_cone/meshes/construction_cone.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<inertial>
<pose>0 0 0 0 -0 0</pose>
<inertia>
<ixx>1</ixx>
<ixy>0</ixy>
<ixz>0</ixz>
<iyy>1</iyy>
<iyz>0</iyz>
<izz>1</izz>
</inertia>
<mass>1</mass>
</inertial>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-4.64786 -0.646522 0 0 -0 0</pose>
</model>
<model name='Construction Barrel_1'>
<link name='link'>
<inertial>
<pose>0 0 0.4 0 -0 0</pose>
<mass>500</mass>
<inertia>
<ixx>51.2096</ixx>
<iyy>51.2096</iyy>
<izz>25</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://construction_barrel/meshes/construction_barrel.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://construction_barrel/meshes/construction_barrel.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-5.3108 -2.23708 0 0 -0 0</pose>
</model>
<model name='Construction Barrel_2'>
<link name='link'>
<inertial>
<pose>0 0 0.4 0 -0 0</pose>
<mass>500</mass>
<inertia>
<ixx>51.2096</ixx>
<iyy>51.2096</iyy>
<izz>25</izz>
<ixy>0</ixy>
<ixz>0</ixz>
<iyz>0</iyz>
</inertia>
</inertial>
<collision name='collision'>
<geometry>
<mesh>
<uri>model://construction_barrel/meshes/construction_barrel.dae</uri>
</mesh>
</geometry>
<max_contacts>10</max_contacts>
<surface>
<contact>
<ode/>
</contact>
<bounce/>
<friction>
<torsional>
<ode/>
</torsional>
<ode/>
</friction>
</surface>
</collision>
<visual name='visual'>
<geometry>
<mesh>
<uri>model://construction_barrel/meshes/construction_barrel.dae</uri>
</mesh>
</geometry>
</visual>
<self_collide>0</self_collide>
<enable_wind>0</enable_wind>
<kinematic>0</kinematic>
</link>
<pose>-2.07692 -4.26198 0 0 -0 0</pose>
</model>
<state world_name='default'>
<sim_time>457 444000000</sim_time>
<real_time>459 829915274</real_time>
<wall_time>1646216792 728719362</wall_time>
<iterations>457444</iterations>
<model name='Construction Barrel'>
<pose>-0.756319 1.39223 0 0 0 -2.4e-05</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-0.756319 1.39223 0 0 0 -2.4e-05</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>3.45865 5.7443 4.40608 1.34644 0.778762 3.14097</acceleration>
<wrench>1729.32 2872.15 2203.04 0 -0 0</wrench>
</link>
</model>
<model name='Construction Barrel_0'>
<pose>1.80014 0.377148 0 0 0 -2.2e-05</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>1.80014 0.377148 0 0 0 -2.2e-05</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>3.45864 5.7443 4.40608 1.34642 0.778778 3.14097</acceleration>
<wrench>1729.32 2872.15 2203.04 0 -0 0</wrench>
</link>
</model>
<model name='Construction Barrel_1'>
<pose>-5.31082 -2.23709 -0 0 -0 -7e-06</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-5.31082 -2.23709 -0 0 -0 -7e-06</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-3.87743 -2.8936 0.727843 -2.17783 0.268134 -3.13538</acceleration>
<wrench>-1938.71 -1446.8 363.922 0 -0 0</wrench>
</link>
</model>
<model name='Construction Barrel_2'>
<pose>-2.07693 -4.26199 -0 0 0 -4e-06</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-2.07693 -4.26199 -0 0 0 -4e-06</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>7.60795 0.965639 1.20183 -2.41278 0.170207 0.001195</acceleration>
<wrench>3803.98 482.819 600.916 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone'>
<pose>-2.19745 -2.25046 -0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-2.19745 -2.25046 -0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 -0 -0.008512 -2.58585 1.13682 3.14159</acceleration>
<wrench>0 -0 -0.008512 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_0'>
<pose>-1.40494 -2.55135 -0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-1.40494 -2.55135 -0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 -0 -0.008512 -2.58585 1.13682 3.14159</acceleration>
<wrench>0 -0 -0.008512 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_1'>
<pose>-0.501986 -2.9745 -0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-0.501986 -2.9745 -0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 -0 -0.008512 -2.58585 1.13682 3.14159</acceleration>
<wrench>0 -0 -0.008512 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_2'>
<pose>2.09724 -2.18232 -0 1e-06 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>2.09724 -2.18232 -0 1e-06 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0 -0 -9.79988 3e-05 2e-05 -0</acceleration>
<wrench>-0 -0 -9.79988 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_3'>
<pose>-3.69879 0.345038 -1e-05 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-3.69879 0.345038 -1e-05 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 -9.8 0 -0 0</acceleration>
<wrench>0 0 -9.8 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_4'>
<pose>-0.406438 -4.23374 -0 0 -1e-06 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-0.406438 -4.23374 -0 0 -1e-06 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0 0 0.008512 2.58585 -1.13682 -3.14159</acceleration>
<wrench>-0 0 0.008512 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_5'>
<pose>2.38815 -3.2576 -0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>2.38815 -3.2576 -0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 -0 -0.008512 -2.58585 1.13682 3.14159</acceleration>
<wrench>0 -0 -0.008512 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_6'>
<pose>2.39316 -4.49605 -0 0 -1e-06 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>2.39316 -4.49605 -0 0 -1e-06 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0 0 0.008512 2.58585 -1.13682 -3.14159</acceleration>
<wrench>-0 0 0.008512 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_7'>
<pose>-3.00495 -2.78053 -0 0 -1e-06 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-3.00495 -2.78053 -0 0 -1e-06 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0 0 0.008512 2.58585 -1.13682 -3.14159</acceleration>
<wrench>-0 0 0.008512 0 -0 0</wrench>
</link>
</model>
<model name='Construction Cone_8'>
<pose>-4.64786 -0.646522 -0 0 -1e-06 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>-4.64786 -0.646522 -0 0 -1e-06 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>-0 0 0.008512 2.58585 -1.13682 -3.14159</acceleration>
<wrench>-0 0 0.008512 0 -0 0</wrench>
</link>
</model>
<model name='ground_plane'>
<pose>0 0 0 0 -0 0</pose>
<scale>1 1 1</scale>
<link name='link'>
<pose>0 0 0 0 -0 0</pose>
<velocity>0 0 0 0 -0 0</velocity>
<acceleration>0 0 0 0 -0 0</acceleration>
<wrench>0 0 0 0 -0 0</wrench>
</link>
</model>
<light name='sun'>
<pose>0 0 10 0 -0 0</pose>
</light>
</state>
<gui fullscreen='0'>
<camera name='user_camera'>
<pose>-15.5892 0.392985 21.6628 0 0.851642 0.028194</pose>
<view_controller>orbit</view_controller>
<projection_type>perspective</projection_type>
</camera>
</gui>
</world>
</sdf>