Launch for spawning Actors in Gazebo

hubero_bringup_gazebo_ros/CMakeLists.txt

@@ -16,6 +16,13 @@ find_package(catkin REQUIRED COMPONENTS
 catkin_package()
 
 ## Install
+install(
+  PROGRAMS
+    scripts/trajectory_spawner.sh
+    scripts/xacro_spawner.sh
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
 # ref: https://github.com/pal-robotics/tiago_robot/blob/kinetic-devel/tiago_bringup/CMakeLists.txt
 foreach(dir launch maps rviz worlds)
     install(DIRECTORY ${dir}/

hubero_bringup_gazebo_ros/README.md

@@ -4,11 +4,11 @@ Package contains `.launch` files, `.pgm` maps and `.world` files for evaluation
 
 ## Prerequisites
 
-Before you try to run exemplary worlds, you need to setup Gazebo simulator with a required set of models (used in provided worlds). Visit [Gazebo setup wiki page](https://github.com/rayvburn/hubero/wiki/Gazebo-setup) and start with setup of `3DGems` model pack (this is required, rest may be optional depending on your system configuration).
+Before you try to run exemplary worlds, you need to set up the Gazebo simulator with a required set of models (used in provided worlds). Visit the [Gazebo setup wiki page](https://github.com/rayvburn/hubero/wiki/Gazebo-setup) and start setting up the `3DGems` model pack (this one is required, others may be optional depending on your system configuration).
 
 ## Run
 
-Before a launch of an exemplary world one must apply both ROS workspace and Gazebo configurations:
+Before the launch of an exemplary world, one must apply both ROS workspace and Gazebo configurations:
 
 ```bash
 source <ROS_WS_DIRECTORY>/devel/setup.bash
@@ -21,26 +21,34 @@ Then, `parking` world, defined for Gazebo, with all necessary Actor interfaces (
 roslaunch hubero_bringup_gazebo_ros example.launch world:=parking rviz:=true
 ```
 
-If all went OK, after few seconds after start you should see `move_base` log:
+If all went OK, after few seconds after the start you should see `move_base` log:
 
 ```console
 [ INFO] [1643660811.668211218, 8.331000000]: odom received!
 ```
 
-**NOTE**: actor names defined in Gazebo `.world` must match names given to `.launch` file.
-
-## Spawn
+## Spawning an actor
 
 The crucial question is: how to spawn an `Actor` with a `ActorPlugin` controller in a custom Gazebo world?
 
-Since Gazebo entities are defined in `.world` file, one must add Actor definition to the Gazebo world. Actor definition consists of:
+### Spawn with a `spawn_model` service
+
+A handy `spawn_actor.launch` was prepared to spawn an actor controlled with the `HuBeRo` plugin in the Gazebo. Once the simulation is running, type:
+
+```sh
+roslaunch hubero_bringup_gazebo_ros spawn_actor.launch actor_name:=<ACTOR_UNIQUE_NAME>
+```
+
+### Spawn directly in the `.world` file
+
+Typically, Gazebo entities are defined in a `.world` file, so one can also add the `Actor` definition directly to the Gazebo world. Actor definition consists of:
 
 - starting pose definition,
 - animation definitions,
 - attached sensor relative pose and definition,
-- plugin used to control Actor.
+- plugin used to control the `Actor`.
 
-Generally, one should use the exemplary worlds definitions as a reference to use actors in their custom worlds. Simply open `living_room.world` or `parking.world` and copy section that is related to a given actor:
+Generally, one should use the exemplary worlds definitions as a reference to add actors to their custom worlds. To accomplish that, simply open `living_room.world` or `parking.world` and copy a section that is related to a given actor:
 
 ```xml
 <actor name="actor1">
@@ -53,13 +61,20 @@ Generally, one should use the exemplary worlds definitions as a reference to use
 </actor>
 ```
 
+**NOTE**: actor names defined in Gazebo `.world` must match names given to the `.launch` file.
+
 ## ROS interface
 
-Actors will not have mobility skills without connection with ROS Navigation stack. Also, Actors will not be able to receive task requests without connection to ROS topics. Use `example.launch` parameter definitions to properly define new Actor interfaces.
+Actors will not have mobility skills without the connection with ROS Navigation stack. Also, Actors will not be able to receive task requests without connection to ROS topics. Use `example.launch` parameter definitions to properly define new Actor interfaces.
 
 ## Gazebo world map
 
-To create an ideal map of the Gazebo world one may use [`pgm_map_creator`](https://github.com/hyfan1116/pgm_map_creator) package. Examples of use are provided in `maps/*.yaml` files.
+To create an ideal map of the Gazebo world one may use `pgm_map_creator`:
+
+- [`hyfan1116/pgm_map_creator`](https://github.com/hyfan1116/pgm_map_creator) with ROS Kinetic and Ubuntu 16.04
+- [`rayvburn/pgm_map_creator`](https://github.com/rayvburn/pgm_map_creator.git) with ROS Melodic and Ubuntu 18.04
+
+Examples of use are provided in `maps/*.yaml` files.
 
 One must remember to place/uncomment `collision_map_creator` plugin definition in the `.world` file:
 
@@ -69,13 +84,13 @@ One must remember to place/uncomment `collision_map_creator` plugin definition i
 
 ## Debugging
 
-`example.launch` provides a possibility to debug Actor controller plugin. First, user should extend `GAZEBO_PLUGIN_PATH`:
+`example.launch` provides a possibility to debug the `Actor` controller plugin. First, the user should extend `GAZEBO_PLUGIN_PATH`:
 
 ```bash
-export GAZEBO_PLUGIN_PATH=<PATH_TO_hubero_gazebo_PACKAGE>/lib:$GAZEBO_PLUGIN_PATH
+export GAZEBO_PLUGIN_PATH=<PATH_TO_THE_ROS_WORKSPACE>/devel/.private/hubero_gazebo/lib:$GAZEBO_PLUGIN_PATH
 ```
 
-Then, run Gazebo world with the required plugins. ROS1 distro should be adjusted by user:
+Then, run the Gazebo world with the required plugins. ROS1 distro should be adjusted by the user:
 
 ```bash
 gazebo -s /opt/ros/kinetic/lib/libgazebo_ros_paths_plugin.so -s /opt/ros/kinetic/lib/libgazebo_ros_api_plugin.so <PATH_TO_WORLD_FILE_DIR>/parking.world

hubero_bringup_gazebo_ros/launch/example.launch

@@ -8,6 +8,9 @@
     <arg name="gui"         default="true"/>
     <arg name="rviz"        default="true"/>
     <arg name="gdb"         default="false"/>
+    <arg name="extra_gazebo_args" default=""/>
+    <arg name="recording"   default="false"/>
+    <arg name="paused"      default="false"/>
 
     <!-- Namespace where communication channels (nodes) of simulated actors will be available -->
     <arg name="main_ns" default="hubero"/>
@@ -25,9 +28,11 @@
     <group unless="$(arg gdb)">
         <include file="$(find gazebo_ros)/launch/empty_world.launch">
             <arg name="world_name" value="$(arg world_file)"/>
-            <arg name="paused" value="false"/>
+            <arg name="extra_gazebo_args" value="$(arg extra_gazebo_args)"/>
+            <arg name="recording" value="$(arg recording)"/>
+            <arg name="paused" value="$(arg paused)"/>
             <arg name="use_sim_time" value="true"/>
-            <arg name="gui" value="true"/>
+            <arg name="gui" value="$(arg gui)"/>
             <arg name="headless" value="false"/>
             <arg name="debug" value="false"/>
             <arg name="verbose" value="true"/>

hubero_bringup_gazebo_ros/launch/spawn_actor.launch

@@ -0,0 +1,53 @@
+<?xml version="1.0"?>
+
+<launch>
+    <!-- Parameters related to the spawned model -->
+    <arg name="actor_name" default="actor1"/>
+    <arg name="pose_x" default="0.0"/>
+    <arg name="pose_y" default="0.0"/>
+    <arg name="pose_z" default="1.03"/>
+    <arg name="pose_roll" default="1.5707"/>
+    <arg name="pose_pitch" default="0.0"/>
+    <arg name="pose_yaw" default="1.5707"/>
+    <arg name="visualize_laser" default="false"/>
+    <arg name="skin_file" default="walk.dae"/>
+
+    <!-- Contents below are partially shared with the `example.launch` -->
+    <!-- Actor's map center pose relative to the world center pose -->
+    <arg name="tf_world_map" default="+0.00 +0.00 +0.00 +0.00 +0.00 +0.00"/>
+    <arg name="shared_map" default="true"/>
+    <arg name="map_frame" default="map"/>
+    <!-- Name of the frame used as a reference in simulator poses etc. usually, frame at the top of the ROS TF tree -->
+    <arg name="simulator_global_frame" default="world"/>
+
+    <!-- Namespace where communication channels (nodes) of simulated actors will be available -->
+    <arg name="main_ns" default="hubero"/>
+
+    <!-- Arbitrary map selected here -->
+    <arg name="map_file" default="$(find hubero_bringup_gazebo_ros)/maps/parking.yaml"/>
+    <arg name="map_bounds" default="[-10, 20, -10, 20]"/>
+
+    <!-- Motion planning configuration -->
+    <arg name="global_planner" default="navfn"/>
+    <arg name="local_planner" default="teb"/>
+
+    <!-- Run ROS interface for the HuBeRo actor operating in Gazebo simulation -->
+    <!-- NOTE: Loading a map & publishing a static TF performed inside the `actor.launch` -->
+    <include file="$(find hubero_ros)/launch/actor.launch">
+        <arg name="actor_name" value="$(arg actor_name)"/>
+        <arg name="main_ns" value="$(arg main_ns)"/>
+        <arg name="map_file" value="$(arg map_file)"/>
+        <arg name="use_shared_map" value="$(arg shared_map)"/>
+        <arg name="map_frame" value="$(arg map_frame)"/>
+        <arg name="simulator_global_frame" value="$(arg simulator_global_frame)"/>
+        <arg name="tf_world_map" value="$(arg tf_world_map)"/>
+        <arg name="map_bounds" value="$(arg map_bounds)"/>
+        <arg name="local_planner" value="$(arg local_planner)"/>
+        <arg name="global_planner" value="$(arg global_planner)"/>
+    </include>
+
+    <!-- Spawn actor in the simulation -->
+    <node type="xacro_spawner.sh" pkg="hubero_bringup_gazebo_ros" name="actor_xacro_spawner" ns="$(arg actor_name)" output="screen"
+        args="$(arg actor_name) $(arg pose_x) $(arg pose_y) $(arg pose_z) $(arg pose_roll) $(arg pose_pitch) $(arg pose_yaw) $(arg visualize_laser) $(arg skin_file)"
+    />
+</launch>

hubero_bringup_gazebo_ros/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>hubero_bringup_gazebo_ros</name>
-  <version>0.6.0</version>
+  <version>0.7.0</version>
   <description>The hubero_bringup_gazebo_ros package</description>
 
   <author email="[email protected]">Jarosław Karwowski</author>

hubero_bringup_gazebo_ros/scripts/trajectory_spawner.sh

@@ -0,0 +1,59 @@
+#!/usr/bin/env bash
+#
+# This is a script for spawning a non-HuBeRo actor (generic Gazebo actor) with a predefined trajectory to be executed
+#
+# NOTE: the contents of the input_file (2nd argument) should be in the following form:
+#
+# <?xml version="1.0"?>
+# <sdf version="1.6">
+# <!-- NOTE: SCRIPTDELETE is placed here as it must be deleted from the resultant .xml to be properly loaded by the Gazebo -->
+# <SCRIPTDELETE xmlns:xacro="http://www.ros.org/wiki/xacro">
+#    <!-- Ref: http://classic.gazebosim.org/tutorials?tut=actor&cat=build_robot -->
+#    <!-- Also, namespace for ROS topics related to the actor -->
+#    <xacro:arg name="actor_name" default="actor" />
+#    <!-- Arg->Property hack -->
+#    <xacro:property name="actor_name" value="$(arg actor_name)" />
+#
+#     <actor name="${actor_name}">
+#       <skin>
+#         <filename>walk.dae</filename>
+#       </skin>
+#       <animation name="walking">
+#         <filename>walk.dae</filename>
+#         <interpolate_x>true</interpolate_x>
+#       </animation>
+#       <script>
+#         <trajectory id="0" type="walking">
+#           <waypoint>
+#             <time>0</time>
+#             <pose>0 2 0 0 0 -1.57</pose>
+#           </waypoint>
+#           <waypoint>
+#             <!--
+#             ...
+#             -->
+#           </waypoint>
+#         </trajectory>
+#       </script>
+#     </actor>
+# </SCRIPTDELETE>
+# </sdf>
+#
+
+# Pass arguments
+actor_name="$1"
+# xacro/URDF file location
+input_file="$2"
+
+# compose arguments to be passed to the xacro
+XACRO_ARGS="actor_name:=${actor_name}"
+
+# deletes lines containing SCRIPTDELETE phrase
+SDF=$(xacro "$input_file" $XACRO_ARGS | sed "/SCRIPTDELETE/d")
+
+# save sdf to file
+SDF_FILE="/tmp/trajectory_spawner_${actor_name}"
+echo $SDF > $SDF_FILE
+
+# spawn coords are defined in the xacro
+rosrun gazebo_ros spawn_model -sdf -file $SDF_FILE -model $actor_name

hubero_bringup_gazebo_ros/scripts/xacro_spawner.sh

@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+#
+# Pass xacro arguments one by one, beware of the order
+actor_name="$1"
+pose_x="$2"
+pose_y="$3"
+pose_z="$4"
+pose_roll="$5"
+pose_pitch="$6"
+pose_yaw="$7"
+visualize_laser="$8"
+skin_file="$9"
+
+# compose arguments to be passed to the xacro
+XACRO_ARGS="actor_name:=${actor_name} pose_x:=${pose_x} pose_y:=${pose_y} pose_z:=${pose_z} pose_roll:=${pose_roll} pose_pitch:=${pose_pitch} pose_yaw:=${pose_yaw} visualize_laser:=${visualize_laser} skin_file:=${skin_file}"
+
+# deletes lines containing SCRIPTDELETE phrase
+SDF=$(xacro "$(rospack find hubero_gazebo)/urdf/actor.xacro" $XACRO_ARGS | sed "/SCRIPTDELETE/d")
+
+# save sdf to file
+SDF_FILE="/tmp/xacro_spawner_${actor_name}"
+echo $SDF > $SDF_FILE
+
+# spawn coords are defined in the xacro
+rosrun gazebo_ros spawn_model -sdf -file $SDF_FILE -model $actor_name

hubero_bringup_gazebo_ros/worlds/parking.world

@@ -3910,12 +3910,6 @@
         <interpolate_x>true</interpolate_x>
       </animation>
 
-      <animation name="stand_up">
-        <filename>stand_up.dae</filename>
-        <scale>1.000000</scale>
-        <interpolate_x>true</interpolate_x>
-      </animation>
-
       <animation name="run">
         <filename>run.dae</filename>
         <scale>1.000000</scale>
@@ -4019,12 +4013,6 @@
         <interpolate_x>true</interpolate_x>
       </animation>
 
-      <animation name="stand_up">
-        <filename>stand_up.dae</filename>
-        <scale>1.000000</scale>
-        <interpolate_x>true</interpolate_x>
-      </animation>
-
       <animation name="run">
         <filename>run.dae</filename>
         <scale>1.000000</scale>