Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Support validating target reports through image stream #59

Merged
merged 6 commits into from
Mar 29, 2022
Merged

Support validating target reports through image stream #59

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
a8f2a9a
implemented target validation, streaming start/stop/report
iche033 Mar 4, 2022
72ed5c6
add test
iche033 Mar 9, 2022
dc8d4c0
update test
iche033 Mar 10, 2022
28876e6
double to int for img pos
iche033 Mar 11, 2022
9c67603
doc
iche033 Mar 11, 2022
008e962
Stop robot motion when it moves out of 2nd geofence (#60)
iche033 Mar 29, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
991 changes: 913 additions & 78 deletions mbzirc_ign/src/GameLogicPlugin.cc
View file
Open in desktop

Large diffs are not rendered by default.

826 changes: 809 additions & 17 deletions mbzirc_ign/test/test_game_logic.cc
View file
Open in desktop

Large diffs are not rendered by default.

7 changes: 7 additions & 0 deletions mbzirc_ign/worlds/empty_platform.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
157 changes: 141 additions & 16 deletions mbzirc_ign/worlds/faster_than_realtime.sdf
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down Expand Up @@ -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">
Expand Down Expand Up @@ -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">
Expand All @@ -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>
Expand Down
5 changes: 5 additions & 0 deletions mbzirc_ros/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
Expand Down Expand Up @@ -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)
Expand All @@ -48,6 +52,7 @@ if(BUILD_TESTING)
ament_target_dependencies(test_ros_api
geometry_msgs
rclcpp
rosgraph_msgs
sensor_msgs
std_msgs
tf2_msgs
Expand Down
70 changes: 70 additions & 0 deletions mbzirc_ros/launch/competition.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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)
])

1 change: 1 addition & 0 deletions mbzirc_ros/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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>
Expand Down
4 changes: 2 additions & 2 deletions mbzirc_ros/test/launch/test_ros_api.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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())

Expand Down
21 changes: 21 additions & 0 deletions mbzirc_ros/test/ros_api/test_ros_api.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down Expand Up @@ -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);
}

/////////////////////////////////////////////////
Expand Down