From 708518e229f62cc9763c8b29abe1e770071c6a5e Mon Sep 17 00:00:00 2001
From: Alejandro Lopez Gestoso <alopez@iti.upc.edu>
Date: Thu, 8 Jul 2021 19:33:56 +0200
Subject: [PATCH] Add rplidar to simulation
---
launch/includes/kobuki.launch.xml | 23 +++++++++++++++
launch/sim.launch | 29 +++++++++++++------
urdf/kobuki_hexagons_laser.urdf.xacro | 20 ++++++++++++++
urdf/laser.gazebo | 40 +++++++++++++++++++++++++++
urdf/laser.urdf.xacro | 40 +++++++++++++++++++++++++++
5 files changed, 144 insertions(+), 8 deletions(-)
create mode 100644 launch/includes/kobuki.launch.xml
create mode 100644 urdf/kobuki_hexagons_laser.urdf.xacro
create mode 100644 urdf/laser.gazebo
create mode 100644 urdf/laser.urdf.xacro
diff --git a/launch/includes/kobuki.launch.xml b/launch/includes/kobuki.launch.xml
new file mode 100644
index 0000000..7872206
--- /dev/null
+++ b/launch/includes/kobuki.launch.xml
@@ -0,0 +1,23 @@
+<launch>
+ <arg name="base"/>
+ <arg name="stacks"/>
+ <arg name="3d_sensor"/>
+
+ <arg name="urdf_file" default="$(find xacro)/xacro '$(find iri_turtlebot_launch)/urdf/$(arg base)_$(arg stacks)_$(arg 3d_sensor).urdf.xacro'"/>
+ <param name="robot_description" command="$(arg urdf_file)"/>
+
+ <!-- Gazebo model spawner -->
+ <node name="spawn_turtlebot_model" pkg="gazebo_ros" type="spawn_model"
+ args="$(optenv ROBOT_INITIAL_POSE) -unpause -urdf -param robot_description -model mobile_base"/>
+
+ <!-- Velocity muxer -->
+ <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
+ <node pkg="nodelet" type="nodelet" name="cmd_vel_mux"
+ args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
+ <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml"/>
+ <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
+ </node>
+
+ <!-- Bumper/cliff to pointcloud (not working, as it needs sensors/core messages) -->
+ <include file="$(find turtlebot_bringup)/launch/includes/kobuki/bumper2pc.launch.xml"/>
+</launch>
diff --git a/launch/sim.launch b/launch/sim.launch
index 1607479..4976c0e 100644
--- a/launch/sim.launch
+++ b/launch/sim.launch
@@ -6,6 +6,7 @@
<arg name="world_description" default="true"/>
<arg name="rviz" default="false"/>
<arg name="gui" default="true"/>
+ <arg name="rplidar" default="true"/>
<arg name="base" value="$(optenv TURTLEBOT_BASE kobuki)"/> <!-- create, roomba -->
<arg name="battery" value="$(optenv TURTLEBOT_BATTERY /proc/acpi/battery/BAT0)"/> <!-- /proc/acpi/battery/BAT0 -->
@@ -20,15 +21,22 @@
<arg name="rviz" value="false"/>
</include>
- <include file="$(find turtlebot_gazebo)/launch/includes/$(arg base).launch.xml">
- <arg name="base" value="$(arg base)"/>
- <arg name="stacks" value="$(arg stacks)"/>
- <arg name="3d_sensor" value="$(arg 3d_sensor)"/>
- </include>
+ <group if="$(arg rplidar)">
+ <include file="$(find iri_turtlebot_launch)/launch/includes/kobuki.launch.xml">
+ <arg name="base" value="kobuki"/>
+ <arg name="stacks" value="hexagons"/>
+ <arg name="3d_sensor" value="laser"/>
+ </include>
+
+ </group>
+
+ <group unless="$(arg rplidar)">
+ <include file="$(find turtlebot_gazebo)/launch/includes/$(arg base).launch.xml">
+ <arg name="base" value="$(arg base)"/>
+ <arg name="stacks" value="$(arg stacks)"/>
+ <arg name="3d_sensor" value="$(arg 3d_sensor)"/>
+ </include>
- <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
- <param name="publish_frequency" type="double" value="30.0" />
- </node>
<!-- Fake laser -->
<node pkg="nodelet" type="nodelet" name="laserscan_nodelet_manager" args="manager"/>
@@ -41,4 +49,9 @@
<remap from="scan" to="/scan"/>
</node>
+ </group>
+
+ <node pkg="robot_state_publisher" type="robot_state_publisher" name="robot_state_publisher">
+ <param name="publish_frequency" type="double" value="30.0" />
+ </node>
</launch>
diff --git a/urdf/kobuki_hexagons_laser.urdf.xacro b/urdf/kobuki_hexagons_laser.urdf.xacro
new file mode 100644
index 0000000..e73ac3f
--- /dev/null
+++ b/urdf/kobuki_hexagons_laser.urdf.xacro
@@ -0,0 +1,20 @@
+<?xml version="1.0"?>
+<!--
+ - Base : kobuki
+ - Stacks : hexagons
+ - 3d Sensor : laser
+-->
+<robot name="turtlebot" xmlns:xacro="http://ros.org/wiki/xacro">
+
+ <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_common_library.urdf.xacro" />
+ <xacro:include filename="$(find kobuki_description)/urdf/kobuki.urdf.xacro" />
+ <xacro:include filename="$(find turtlebot_description)/urdf/stacks/hexagons.urdf.xacro"/>
+ <!--<xacro:include filename="$(find turtlebot_description)/urdf/sensors/kinect.urdf.xacro"/>-->
+
+ <xacro:include filename="$(find iri_turtlebot_launch)/urdf/laser.urdf.xacro"/>
+
+ <kobuki/>
+ <stack_hexagons parent="base_link"/>
+ <!--<sensor_kinect parent="base_link"/>-->
+ <sensor_laser parent="plate_middle_link"/>
+</robot>
diff --git a/urdf/laser.gazebo b/urdf/laser.gazebo
new file mode 100644
index 0000000..d7810be
--- /dev/null
+++ b/urdf/laser.gazebo
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot name="sensor_laser" xmlns:xacro="http://ros.org/wiki/xacro">
+ <xacro:macro name="laser">
+ <gazebo reference="laser">
+ <sensor type="ray" name="laser_sensor">
+ <pose>0 0 0 0 0 0</pose>
+ <visualize>false</visualize>
+ <update_rate>10</update_rate>
+ <ray>
+ <scan>
+ <horizontal>
+ <samples>400</samples>
+ <resolution>0.25</resolution>
+ <min_angle>-3.141592654</min_angle>
+ <max_angle>3.141592654</max_angle>
+ </horizontal>
+ </scan>
+ <range>
+ <min>0.10</min>
+ <max>6.0</max>
+ <resolution>0.01</resolution>
+ </range>
+ <noise>
+ <type>gaussian</type>
+ <!-- Noise parameters based on published spec for laser laser
+ achieving "+-30mm" accuracy at range < 10m. A mean of 0.0m and
+ stddev of 0.01m will put 99.7% of samples within 0.03m of the true
+ reading. -->
+ <mean>0.0</mean>
+ <stddev>0.005</stddev>
+ </noise>
+ </ray>
+ <plugin name="gazebo_laser_controller" filename="libgazebo_ros_laser.so">
+ <topicName>scan</topicName>
+ <frameName>laser</frameName>
+ </plugin>
+ </sensor>
+ </gazebo>
+ </xacro:macro>
+</robot>
diff --git a/urdf/laser.urdf.xacro b/urdf/laser.urdf.xacro
new file mode 100644
index 0000000..c85b598
--- /dev/null
+++ b/urdf/laser.urdf.xacro
@@ -0,0 +1,40 @@
+<?xml version="1.0"?>
+<robot name="sensor_laser" xmlns:xacro="http://ros.org/wiki/xacro">
+ <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_gazebo.urdf.xacro"/>
+ <xacro:include filename="$(find turtlebot_description)/urdf/turtlebot_properties.urdf.xacro"/>
+
+ <xacro:include filename="$(find iri_turtlebot_launch)/urdf/laser.gazebo"/>
+
+ <xacro:property name="size" value="0.05"/>
+
+ <xacro:macro name="sensor_laser" params="parent">
+ <link name="laser">
+ <collision>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <box size="${size} ${size} ${size}"/>
+ </geometry>
+ </collision>
+ <visual>
+ <origin xyz="0 0 0" rpy="0 0 0"/>
+ <geometry>
+ <box size="${size} ${size} ${size}"/>
+ </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="laser_joint" type="fixed">
+ <!--<axis xyz="0 0 1" />-->
+ <origin xyz="0 0 0.03" rpy="0 0 0"/>
+ <parent link="${parent}"/>
+ <child link="laser"/>
+ </joint>
+ <!-- Laser sensor for simulation -->
+ <laser/>
+ </xacro:macro>
+</robot>
+
--
GitLab