Skip to content

Commit

Permalink
Cave feature release2 (#462)
Browse files Browse the repository at this point in the history 
* Add falling rocks to cave practice circuit 1 (#446)

* Add rock fall 1

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Add rock fall 2

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Add rock fall 3

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Change small rock fall to medium, update positions of large rock falls

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>

* Add falling rocks to cave practice circuit 2 (#447)

* Adjustment of fading exponent from 2.5 to 1.5 for the cave circuit.

* Add fading exponent to cloudsim launch files

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added dynamic rocks to cave practice world 2

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Remove debug output

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Fix rock fall positions and types

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Comments

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Fix medium rock placements

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Added rock fall, moved rocks out of collision

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Doubled two rock falls, and reduced one to 3 deployments

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Arthur Schang <arthur.schang@parsons.com>
Co-authored-by: Nate Koenig <nate@openrobotics.org>

* Add falling rocks to cave practice 3 (#448)

* Add falling rocks to cave practice 3

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Comments

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Move rocks out of collision

Signed-off-by: Nate Koenig <nate@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>

* Fix levels

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Build the connection validator test (#455)

* Build the connection validator test

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Improve debug message

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Removed probably

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Make connection validator an executable

Signed-off-by: Nate Koenig <nate@openrobotics.org>

* Update subt_ign/src/ConnectionValidatorPrivate.cc

Co-authored-by: Michael Carroll <michael@openrobotics.org>

* Use a wider cap tolerance as a fallback

This is because some of the cave caps have a non-zero connection point,
but we do not propagate enough information to be able to compute the
true connection point at this time.

Signed-off-by: Michael Carroll <michael@openrobotics.org>

Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Michael Carroll <michael@openrobotics.org>

Co-authored-by: Addisu Taddese <addisu@openrobotics.org>
Co-authored-by: Arthur Schang <arthur.schang@parsons.com>
Co-authored-by: Nate Koenig <nate@openrobotics.org>
Co-authored-by: Michael Carroll <michael@openrobotics.org>
  • Loading branch information
@nkoenig @azeey
@acschang @mjcarroll
5 people authored Jun 10, 2020
1 parent 69a4364 commit db5344b
Show file tree
Hide file tree
Showing 8 changed files with 694 additions and 11 deletions.
24 changes: 24 additions & 0 deletions subt_ign/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -153,6 +153,30 @@ install(TARGETS ${connection_validator_name}
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

add_executable(validate_connections
src/validate_connections.cc
src/ConnectionValidator.cc
src/ConnectionValidatorPrivate.cc
src/ConnectionHelper.cc
src/SimpleDOTParser.cc)
target_link_libraries(validate_connections
PRIVATE
ignition-gazebo${IGN_GAZEBO_VER}::core
ignition-common3::ignition-common3
ignition-launch1::ignition-launch1
ignition-math6::ignition-math6
ignition-msgs4::ignition-msgs4
ignition-plugin1::loader
ignition-transport7::ignition-transport7
${catkin_LIBRARIES}
${protobuf_lib_name}
)
install(TARGETS validate_connections
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)


# Create the libCommsBrokerPlugin.so library.
set(comms_broker_plugin_name CommsBrokerPlugin)
add_library(${comms_broker_plugin_name}
Expand Down
10 changes: 7 additions & 3 deletions subt_ign/launch/cave_circuit.ign
View file
Original file line number Diff line number Diff line change
Expand Up @@ -196,9 +196,13 @@
<%
# disable levels for simple cave worlds
levels = "true"
$levels = "true"
if $worldName.include?('simple_cave_')
levels = "false"
$levels = "false"
end
if defined?(levels) && levels != nil && !levels.empty?
$levels = levels.downcase == "true"
end
%>
Expand Down Expand Up @@ -232,7 +236,7 @@
<update_rate><%= updateRate %></update_rate>
<%end%>
<run>true</run>
<levels><%= levels %></levels>
<levels><%= $levels %></levels>
<record>
<enabled>true</enabled>
<!-- This path is used by cloudsim, please do not change -->
Expand Down
10 changes: 7 additions & 3 deletions subt_ign/launch/cloudsim_sim.ign
View file
Original file line number Diff line number Diff line change
Expand Up @@ -208,9 +208,13 @@
<%
# disable levels for simple cave worlds
levels = "true"
$levels = "true"
if $worldName.include?('simple_cave_')
levels = "false"
$levels = "false"
end
if defined?(levels) && levels != nil && !levels.empty?
$levels = levels.downcase == "true"
end
%>
Expand Down Expand Up @@ -238,7 +242,7 @@
<update_rate><%= updateRate %></update_rate>
<%end%>
<run>true</run>
<levels><%= levels %></levels>
<levels><%= $levels %></levels>
<record>
<enabled>true</enabled>
<!-- This path is used by cloudsim, please do not change -->
Expand Down
15 changes: 12 additions & 3 deletions subt_ign/src/ConnectionValidatorPrivate.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -327,18 +327,27 @@ void ConnectionValidatorPrivate::PopulateConnections()
{
for (auto cap: caps)
{
if (point.Equal(cap, 1))
if (point.Equal(cap, 1.0))
{
igndbg << "Found cap!" << std::endl;
found_caps += 1;
}
else if (point.Equal(cap, 12.6))
{
igndbg << "Found cap (with expanded tolerance)" << std::endl;
found_caps += 1;
}
}
}

if (actual + found_caps != data)
{
igndbg << name << " " << actualConnections[name] << "/" << data <<
" (" << this->vertData[name].tileType << ")" << std::endl;
igndbg << "Actual connections[" << actualConnections[name]
<< "] do not match expected connections[" << data
<< "] for tile name[" << name << "] with type["
<< this->vertData[name].tileType << "]. "
<< "Missing an end cap or connecting tile."
<< std::endl;
}
}
}
Expand Down
4 changes: 2 additions & 2 deletions subt_ign/src/ConnectionValidator_TEST.cc → subt_ign/src/validate_connections.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,8 +15,8 @@
*
*/

/// Helper test to evaluate graph population without starting gazebo.
/// Call with TEST_connection_validator <world_name>
/// Helper to evaluate graph population without starting gazebo.
/// Call with `validate_connections` <world_name>
#include "ConnectionValidatorPrivate.hh"

#include <ignition/common/Console.hh>
Expand Down
134 changes: 134 additions & 0 deletions subt_ign/worlds/cave_circuit_practice_01.sdf
View file
Original file line number Diff line number Diff line change
Expand Up @@ -447,6 +447,140 @@
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Samsung J8 Black</uri>
</include>

<model name="performer_detector_1">
<static>true</static>
<pose>287.47 -105.5 -19.67 0 0 0</pose>
<!--Uncomment <link> to preview detector region-->
<!--<link name='body'>-->
<!-- <visual name="v1">-->
<!-- <transparency>0.5</transparency>-->
<!-- <geometry>-->
<!-- <box>-->
<!-- <size>20 30 20</size>-->
<!-- </box>-->
<!-- </geometry>-->
<!-- </visual>-->
<!-- <material>-->
<!-- <ambient>0.0 1.0 0.0 1</ambient>-->
<!-- <diffuse>0.0 1.0 0.0 1</diffuse>-->
<!-- <specular>0.5 0.5 0.5 1</specular>-->
<!-- </material>-->
<!-- <cast_shadows>false</cast_shadows>-->
<!--</link>-->
<plugin filename="libignition-gazebo-performer-detector-system.so"
name="ignition::gazebo::systems::PerformerDetector">
<topic>/subt_performer_detector</topic>
<geometry>
<box>
<size>20 30 20</size>
</box>
</geometry>
</plugin>

<!--Triggered by performer_detector_1-->
<plugin name="ignition::gazebo::systems::TriggeredPublisher" filename="libignition-gazebo-triggered-publisher-system.so">
<input type="ignition.msgs.Pose" topic="/subt_performer_detector">
<match field='header.data'>{ key: "frame_id" value: "performer_detector_1" }</match>
<match field='header.data'>{ key: "state" value: "1" }</match>
</input>
<output type="ignition.msgs.Empty" topic="/model/medium_rock_fall_1/breadcrumbs/Rock/deploy"/>
</plugin>
</model>
<include>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Medium Rock Fall</uri>
<name>medium_rock_fall_1</name>
<pose>284.95 -104.381 -15.65 0 -1.0147 0</pose>
</include>
<model name="performer_detector_2">
<static>true</static>
<pose>101 -296 4 0 0 0</pose>
<!--Uncomment <link> to preview detector region-->
<!--<link name='body'>-->
<!-- <visual name="v1">-->
<!-- <transparency>0.5</transparency>-->
<!-- <geometry>-->
<!-- <box>-->
<!-- <size>16 16 15</size>-->
<!-- </box>-->
<!-- </geometry>-->
<!-- </visual>-->
<!-- <material>-->
<!-- <ambient>0.0 1.0 0.0 1</ambient>-->
<!-- <diffuse>0.0 1.0 0.0 1</diffuse>-->
<!-- <specular>0.5 0.5 0.5 1</specular>-->
<!-- </material>-->
<!-- <cast_shadows>false</cast_shadows>-->
<!--</link>-->
<plugin filename="libignition-gazebo-performer-detector-system.so"
name="ignition::gazebo::systems::PerformerDetector">
<topic>/subt_performer_detector</topic>
<geometry>
<box>
<size>16 16 15</size>
</box>
</geometry>
</plugin>

<!--Triggered by performer_detector_2-->
<plugin name="ignition::gazebo::systems::TriggeredPublisher" filename="libignition-gazebo-triggered-publisher-system.so">
<input type="ignition.msgs.Pose" topic="/subt_performer_detector">
<match field='header.data'>{ key: "frame_id" value: "performer_detector_2" }</match>
<match field='header.data'>{ key: "state" value: "0" }</match>
</input>
<output type="ignition.msgs.Empty" topic="/model/large_rock_fall_1/breadcrumbs/Rock/deploy"/>
</plugin>
</model>
<include>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall</uri>
<name>large_rock_fall_1</name>
<pose>102.692000 -295.402000 3.005300 0 0.402990 -0.915400</pose>
</include>

<model name="performer_detector_3">
<static>true</static>
<pose>178 -270 -45 0 0 0</pose>
<!--Uncomment <link> to preview detector region-->
<!--<link name='body'>-->
<!-- <visual name="v1">-->
<!-- <transparency>0.5</transparency>-->
<!-- <geometry>-->
<!-- <box>-->
<!-- <size>16 16 15</size>-->
<!-- </box>-->
<!-- </geometry>-->
<!-- </visual>-->
<!-- <material>-->
<!-- <ambient>0.0 1.0 0.0 1</ambient>-->
<!-- <diffuse>0.0 1.0 0.0 1</diffuse>-->
<!-- <specular>0.5 0.5 0.5 1</specular>-->
<!-- </material>-->
<!-- <cast_shadows>false</cast_shadows>-->
<!--</link>-->
<plugin filename="libignition-gazebo-performer-detector-system.so"
name="ignition::gazebo::systems::PerformerDetector">
<topic>/subt_performer_detector</topic>
<geometry>
<box>
<size>16 16 15</size>
</box>
</geometry>
</plugin>

<!--Triggered by performer_detector_2-->
<plugin name="ignition::gazebo::systems::TriggeredPublisher" filename="libignition-gazebo-triggered-publisher-system.so">
<input type="ignition.msgs.Pose" topic="/subt_performer_detector">
<match field='header.data'>{ key: "frame_id" value: "performer_detector_3" }</match>
<match field='header.data'>{ key: "state" value: "0" }</match>
</input>
<output type="ignition.msgs.Empty" topic="/model/large_rock_fall_2/breadcrumbs/Rock/deploy"/>
</plugin>
</model>
<include>
<uri>https://fuel.ignitionrobotics.org/1.0/OpenRobotics/models/Large Rock Fall</uri>
<name>large_rock_fall_2</name>
<pose>178.14 -270 -47.83 0.210254 0 1.57</pose>
</include>

<!-- Auto generated: level_generator 161 182 140 40 -->
<plugin name="ignition::gazebo" filename="dummy">
<level name="level0">
Expand Down
Loading

0 comments on commit db5344b

Please sign in to comment.