Support validating target reports through image stream (#59)

* implemented target validation, streaming start/stop/report Signed-off-by: Ian Chen <[email protected]> * add test Signed-off-by: Ian Chen <[email protected]> * update test Signed-off-by: Ian Chen <[email protected]> * double to int for img pos Signed-off-by: Ian Chen <[email protected]> * doc Signed-off-by: Ian Chen <[email protected]> * Stop robot motion when it moves out of 2nd geofence (#60) * disable robot when it moves out of outer geofence Signed-off-by: Ian Chen <[email protected]> * make robot static instead of halting its motion Signed-off-by: Ian Chen <[email protected]> * fix test Signed-off-by: Ian Chen <[email protected]> * update comments Signed-off-by: Ian Chen <[email protected]> * Bridge competition topics (#62) * publish phase, add competition launch file Signed-off-by: Ian Chen <[email protected]> * fix and add test Signed-off-by: Ian Chen <[email protected]>
osrf · Mar 29, 2022 · ba6d75e · ba6d75e
1 parent 51238a4
commit ba6d75e
Show file tree

Hide file tree

Showing 9 changed files with 1,969 additions and 113 deletions.
diff --git a/mbzirc_ign/src/GameLogicPlugin.cc b/mbzirc_ign/src/GameLogicPlugin.cc
diff --git a/mbzirc_ign/test/test_game_logic.cc b/mbzirc_ign/test/test_game_logic.cc
diff --git a/mbzirc_ign/worlds/empty_platform.sdf b/mbzirc_ign/worlds/empty_platform.sdf
@@ -93,5 +93,12 @@
       </link>
     </model>
 
+    <!-- The MBZIRC competition logic plugin -->
+    <plugin filename="libGameLogicPlugin.so"
+            name="mbzirc::GameLogicPlugin">
+      <run_duration_seconds>60</run_duration_seconds>
+      <setup_duration_seconds>30</setup_duration_seconds>
+    </plugin>
+
   </world>
 </sdf>
diff --git a/mbzirc_ign/worlds/faster_than_realtime.sdf b/mbzirc_ign/worlds/faster_than_realtime.sdf
@@ -4,16 +4,29 @@
 -->
 <sdf version="1.6">
   <world name="faster_than_realtime">
+    <physics name="4ms" type="dart">
+      <max_step_size>0.004</max_step_size>
+      <real_time_factor>0.0</real_time_factor>
+    </physics>
+
     <plugin
       filename="ignition-gazebo-physics-system"
       name="ignition::gazebo::systems::Physics">
-      <max_step_size>0.02</max_step_size>
-      <real_time_factor>0</real_time_factor>
     </plugin>
     <plugin
       filename="ignition-gazebo-user-commands-system"
       name="ignition::gazebo::systems::UserCommands">
     </plugin>
+    <plugin
+      filename="ignition-gazebo-sensors-system"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre2</render_engine>
+    </plugin>
+    <plugin
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
+    </plugin>
+
     <scene>
       <sky></sky>
       <grid>false</grid>
@@ -56,25 +69,11 @@
             <ambient>0.8 0.8 0.8 1</ambient>
             <diffuse>0.8 0.8 0.8 1</diffuse>
             <specular>0.8 0.8 0.8 1</specular>
-              <pbr>
-                  <metal>
-                      <albedo_map>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Type A/tip/files/materials/textures/Gravel_Albedo.jpg</albedo_map>
-                      <normal_map>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Type A/tip/files/materials/textures/Gravel_Normal.jpg</normal_map>
-                      <roughness_map>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Cave Straight Type A/tip/files/materials/textures/Gravel_Roughness.jpg</roughness_map>
-                  </metal>
-              </pbr>
           </material>
         </visual>
       </link>
     </model>
 
-    <include>
-      <pose>0 0 5 0 0 0</pose>
-      <uri>
-        https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Fortress heightmap
-      </uri>
-    </include>
-
     <model name="heightmap_bounds">
       <static>true</static>
       <link name="link">
@@ -129,6 +128,127 @@
       </uri>
     </include>
 
+    <include>
+      <name>Vessel A</name>
+      <pose>25 25 0.3 0 0.0 -1.0</pose>
+      <uri>https://fuel.ignitionrobotics.org/1.0/openrobotics/models/Vessel A</uri>
+      <plugin
+        filename="libSurface.so"
+        name="ignition::gazebo::systems::Surface">
+        <link_name>link</link_name>
+        <vehicle_length>6</vehicle_length>
+        <vehicle_width>3.3</vehicle_width>
+        <hull_radius>0.27</hull_radius>
+        <fluid_level>0.45</fluid_level>
+
+        <!-- Waves -->
+        <wavefield>
+          <size>1000 1000</size>
+          <cell_count>50 50</cell_count>
+          <wave>
+            <model>PMS</model>
+            <period>5</period>
+            <number>3</number>
+            <scale>1.5</scale>
+            <gain>0.3</gain>
+            <direction>1 0</direction>
+            <angle>0.4</angle>
+            <tau>2.0</tau>
+            <amplitude>0.0</amplitude>
+            <steepness>0.0</steepness>
+          </wave>
+        </wavefield>
+      </plugin>
+
+      <plugin
+        filename="libSimpleHydrodynamics.so"
+        name="ignition::gazebo::systems::SimpleHydrodynamics">
+        <link_name>link</link_name>
+        <!-- Added mass -->
+        <xDotU>0.0</xDotU>
+        <yDotV>0.0</yDotV>
+        <nDotR>0.0</nDotR>
+        <!-- Linear and quadratic drag -->
+        <xU>51.3</xU>
+        <xUU>72.4</xUU>
+        <yV>40.0</yV>
+        <yVV>0.0</yVV>
+        <zW>500.0</zW>
+        <kP>50.0</kP>
+        <mQ>50.0</mQ>
+        <nR>400.0</nR>
+        <nRR>0.0</nRR>
+      </plugin>
+    </include>
+
+    <model name="small_box_a">
+      <pose>24 26.5 0.5 0 0 0</pose>
+      <link name="link">
+        <inertial>
+          <mass>1</mass>
+          <inertia>
+          <ixx>0.0066667</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.0066667</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.0066667</izz>
+          </inertia>
+        </inertial>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.2 0.2 0.2</size>
+            </box>
+          </geometry>
+          <material>
+            <diffuse>0 0 1</diffuse>
+          </material>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.2 0.2 0.2</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
+    <model name="large_box_a">
+      <pose>24.9 24.85 0.65 0 0 0</pose>
+      <link name="link">
+        <inertial>
+          <mass>10</mass>
+          <inertia>
+          <ixx>0.26667</ixx>
+          <ixy>0.0</ixy>
+          <ixz>0.0</ixz>
+          <iyy>0.26667</iyy>
+          <iyz>0.0</iyz>
+          <izz>0.26667</izz>
+          </inertia>
+        </inertial>
+        <visual name="visual">
+          <geometry>
+            <box>
+              <size>0.4 0.4 0.4</size>
+            </box>
+          </geometry>
+          <material>
+            <diffuse>0 1 0</diffuse>
+          </material>
+        </visual>
+        <collision name="collision">
+          <geometry>
+            <box>
+              <size>0.4 0.4 0.4</size>
+            </box>
+          </geometry>
+        </collision>
+      </link>
+    </model>
+
     <!-- The MBZIRC competition logic plugin -->
     <plugin filename="libGameLogicPlugin.so"
             name="mbzirc::GameLogicPlugin">
@@ -141,6 +261,11 @@
         <center>0 0 48.46</center>
         <size>140 140 146.92</size>
       </geofence>
+      <target>
+        <vessel>Vessel A</vessel>
+        <small_object>small_box_a</small_object>
+        <large_object>large_box_a</large_object>
+      </target>
     </plugin>
 
   </world>

diff --git a/mbzirc_ros/CMakeLists.txt b/mbzirc_ros/CMakeLists.txt
@@ -6,6 +6,7 @@ find_package(ament_cmake REQUIRED)
 
 find_package(geometry_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
+find_package(rosgraph_msgs REQUIRED)
 find_package(tf2 REQUIRED)
 find_package(tf2_ros REQUIRED)
 find_package(tf2_msgs REQUIRED)
@@ -34,6 +35,9 @@ install(TARGETS
   optical_frame_publisher
   pose_tf_broadcaster
   DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY
+  launch
+  DESTINATION share/${PROJECT_NAME})
 
 
 if(BUILD_TESTING)
@@ -48,6 +52,7 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_ros_api
     geometry_msgs
     rclcpp
+    rosgraph_msgs
     sensor_msgs
     std_msgs
     tf2_msgs

diff --git a/mbzirc_ros/launch/competition.launch.py b/mbzirc_ros/launch/competition.launch.py
@@ -0,0 +1,70 @@
+# Copyright 2022 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.
+
+from ament_index_python.packages import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
+from launch.actions import ExecuteProcess
+from launch.actions import OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration
+
+from launch_ros.actions import Node
+
+import os
+
+def launch(context, *args, **kwargs):
+
+    ign_args = LaunchConfiguration('ign_args').perform(context)
+
+    ign_gazebo = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource([os.path.join(
+        get_package_share_directory('ros_ign_gazebo'), 'launch'),
+        '/ign_gazebo.launch.py']),
+        launch_arguments = {'ign_args': ign_args}.items())
+
+    ros2_ign_score_bridge = Node(
+        package='ros_ign_bridge',
+        executable='parameter_bridge',
+        output='screen',
+        arguments=['/mbzirc/score@std_msgs/msg/[email protected]'],
+    )
+
+    ros2_ign_run_clock_bridge = Node(
+        package='ros_ign_bridge',
+        executable='parameter_bridge',
+        output='screen',
+        arguments=['/mbzirc/run_clock@rosgraph_msgs/msg/[email protected]'],
+    )
+
+    ros2_ign_phase_bridge = Node(
+        package='ros_ign_bridge',
+        executable='parameter_bridge',
+        output='screen',
+        arguments=['/mbzirc/phase@std_msgs/msg/[email protected]'],
+    )
+
+    return [ign_gazebo,
+            ros2_ign_score_bridge,
+            ros2_ign_run_clock_bridge,
+            ros2_ign_phase_bridge]
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument('ign_args', default_value='',
+            description='Arguments to be passed to Ignition Gazebo'),
+        OpaqueFunction(function = launch)
+        ])
+
diff --git a/mbzirc_ros/package.xml b/mbzirc_ros/package.xml
@@ -14,6 +14,7 @@
 
   <depend>geometry_msgs</depend>
   <depend>rclcpp</depend>
+  <depend>rosgraph_msgs</depend>
   <depend>sensor_msgs</depend>
   <depend>tf2</depend>
   <depend>tf2_ros</depend>

diff --git a/mbzirc_ros/test/launch/test_ros_api.launch.py b/mbzirc_ros/test/launch/test_ros_api.launch.py
@@ -45,8 +45,8 @@ def generate_test_description():
     gazebo = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             os.path.join(
-                get_package_share_directory('ros_ign_gazebo'),
-                'launch/ign_gazebo.launch.py')
+                get_package_share_directory('mbzirc_ros'),
+                'launch/competition.launch.py')
         ),
         launch_arguments=arguments.items())
 

diff --git a/mbzirc_ros/test/ros_api/test_ros_api.cc b/mbzirc_ros/test/ros_api/test_ros_api.cc
@@ -18,12 +18,15 @@
 #include <gtest/gtest.h>
 
 #include <rclcpp/rclcpp.hpp>
+#include <rosgraph_msgs/msg/clock.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
 #include <sensor_msgs/msg/fluid_pressure.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/imu.hpp>
 #include <sensor_msgs/msg/magnetic_field.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
+#include <std_msgs/msg/float32.hpp>
+#include <std_msgs/msg/string.hpp>
 #include <tf2_msgs/msg/tf_message.hpp>
 
 using std::placeholders::_1;
@@ -105,6 +108,24 @@ TEST(RosApiTest, SimTopics)
   waitUntilBoolVarAndSpin(
     node, tfStatic.callbackExecuted, 10ms, 3500);
   EXPECT_TRUE(tfStatic.callbackExecuted);
+
+  // score
+  MyTestClass<std_msgs::msg::Float32> score("/mbzirc/score");
+  waitUntilBoolVarAndSpin(
+    node, score.callbackExecuted, 10ms, 500);
+  EXPECT_TRUE(score.callbackExecuted);
+
+  // phase
+  MyTestClass<std_msgs::msg::String> phase("/mbzirc/phase");
+  waitUntilBoolVarAndSpin(
+    node, phase.callbackExecuted, 10ms, 500);
+  EXPECT_TRUE(phase.callbackExecuted);
+
+  // run_clock
+  MyTestClass<rosgraph_msgs::msg::Clock> runClock("/mbzirc/run_clock");
+  waitUntilBoolVarAndSpin(
+    node, runClock.callbackExecuted, 10ms, 500);
+  EXPECT_TRUE(runClock.callbackExecuted);
 }
 
 /////////////////////////////////////////////////