search

osrf / vrx

Virtual RobotX (VRX) resources.
Apache License 2.0
390 stars 180 forks source link

Adding wildlife publication rate and rearrange animals #634

Closed caguero closed 1 year ago

caguero commented 1 year ago

Fixes #604 and #605 .

This pull request adds a new parameter to the wildlife scoring plugin to control the animal publication update rate (simulation time). Now, the default update rate is 1 Hz. Practice 1 and 2 are configured at 0.33 Hz and 0.1 Hz respectively.

As a bonus, I've also modified practice 2 and 3 worlds to use two turtles and two platypus respectively.

How to test it?

Launch a wildlife world. E.g.:

ros2 launch vrx_gz competition.launch.py world:=practice_2022_wildlife1_task

Open a new terminal and run:

ros2 topic hz /vrx/wildlife/animal0/pose

You should get something similar to this:

average rate: 0.249
    min: 3.942s max: 4.085s std dev: 0.05929s window: 3

Note that the expected value should be 0.33 but my real time factor is around 75%. That's why I'm getting 0.25 instead.

You can also circumnavigate the robot around the animals and confirm that the circumnavigation detection code still works.

ros2 launch vrx_gz usv_joy_teleop.py

Look for this message in the simulation terminal:

[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:329] turtle2::link Transition from NEVER_ENGAGED to ENGAGED
[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:380] turtle2::link Virtual gate crossed counterclockwise! (12% completed)
[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:380] turtle2::link Virtual gate crossed counterclockwise! (25% completed)
[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:380] turtle2::link Virtual gate crossed counterclockwise! (37% completed)
[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:380] turtle2::link Virtual gate crossed counterclockwise! (50% completed)
[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:380] turtle2::link Virtual gate crossed counterclockwise! (62% completed)
[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:380] turtle2::link Virtual gate crossed counterclockwise! (75% completed)
[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:380] turtle2::link Virtual gate crossed counterclockwise! (87% completed)
[ruby $(which gz) sim-1] [Dbg] [WildlifeScoringPlugin.cc:380] turtle2::link Virtual gate crossed counterclockwise! (100% completed)
M1chaelM commented 1 year ago

@caguero The hz seems to be working for me. Regarding the "bonus," could we make a world with two crocodiles "guarding" a platypus? Something like:

           wamv

<-----croc------>

             <-----croc------>

             ^
          platypus
             v
caguero commented 1 year ago

3d42fe7

Here's the bodyguard configuration 3d42fe7 :)

M1chaelM commented 1 year ago

@caguero Does this work on your machine? I'm getting a crash a few seconds after OnRunning begins.

[ruby $(which gz) sim-1] Stack trace (most recent call last):
[ruby $(which gz) sim-1] #31   Object "[0xffffffffffffffff]", at 0xffffffffffffffff, in 
[ruby $(which gz) sim-1] #30   Object "gz sim server", at 0x56087a59f1c4, in _start
[ruby $(which gz) sim-1] #29   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fbc1dcd8e3f, in __libc_start_main
[ruby $(which gz) sim-1] #28   Object "/lib/x86_64-linux-gnu/libc.so.6", at 0x7fbc1dcd8d8f, in 
[ruby $(which gz) sim-1] #27   Object "gz sim server", at 0x56087a59f17e, in 
[ruby $(which gz) sim-1] #26   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1df7fe19, in ruby_run_node
[ruby $(which gz) sim-1] #25   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1df7c317, in 
[ruby $(which gz) sim-1] #24   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e11130c, in rb_vm_exec
[ruby $(which gz) sim-1] #23   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e10bc96, in 
[ruby $(which gz) sim-1] #22   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e108fc5, in 
[ruby $(which gz) sim-1] #21   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e106c34, in 
[ruby $(which gz) sim-1] #20   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e052a1e, in 
[ruby $(which gz) sim-1] #19   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1df7d9ac, in rb_protect
[ruby $(which gz) sim-1] #18   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e115c61, in rb_yield
[ruby $(which gz) sim-1] #17   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e11130c, in rb_vm_exec
[ruby $(which gz) sim-1] #16   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e10bc96, in 
[ruby $(which gz) sim-1] #15   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e108fc5, in 
[ruby $(which gz) sim-1] #14   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e106c34, in 
[ruby $(which gz) sim-1] #13   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7fbc19e6244b, in 
[ruby $(which gz) sim-1] #12   Object "/lib/x86_64-linux-gnu/libruby-3.0.so.3.0", at 0x7fbc1e0d4088, in rb_nogvl
[ruby $(which gz) sim-1] #11   Object "/usr/lib/x86_64-linux-gnu/ruby/3.0.0/fiddle.so", at 0x7fbc19e61d6b, in 
[ruby $(which gz) sim-1] #10   Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7fbc19e53492, in 
[ruby $(which gz) sim-1] #9    Object "/lib/x86_64-linux-gnu/libffi.so.8", at 0x7fbc19e56e2d, in 
[ruby $(which gz) sim-1] #8    Object "/home/mrmccarr@ern.nps.edu/gazebo_ws/install/lib/libgz-sim7-gz.so.7.0.0", at 0x7fbc1936596a, in runServer
[ruby $(which gz) sim-1] #7    Object "/home/mrmccarr@ern.nps.edu/gazebo_ws/install/lib/libgz-sim7.so.7", at 0x7fbc18fc499c, in 
[ruby $(which gz) sim-1] #6    Object "/home/mrmccarr@ern.nps.edu/gazebo_ws/install/lib/libgz-sim7.so.7", at 0x7fbc18fd763a, in gz::sim::v7::SimulationRunner::Run(unsigned long)
[ruby $(which gz) sim-1] #5    Object "/home/mrmccarr@ern.nps.edu/gazebo_ws/install/lib/libgz-sim7.so.7", at 0x7fbc18fd6dd0, in gz::sim::v7::SimulationRunner::Step(gz::sim::v7::UpdateInfo const&)
[ruby $(which gz) sim-1] #4    Object "/home/mrmccarr@ern.nps.edu/gazebo_ws/install/lib/libgz-sim7.so.7", at 0x7fbc18fcd7b1, in gz::sim::v7::SimulationRunner::UpdateSystems()
[ruby $(which gz) sim-1] #3    Object "/home/mrmccarr@ern.nps.edu/vrx_ign/install/lib/libWildlifeScoringPlugin.so", at 0x7fbbe575ce15, in vrx::WildlifeScoringPlugin::PreUpdate(gz::sim::v7::UpdateInfo const&, gz::sim::v7::EntityComponentManager&)
[ruby $(which gz) sim-1] #2    Object "/home/mrmccarr@ern.nps.edu/vrx_ign/install/lib/libWildlifeScoringPlugin.so", at 0x7fbbe575e351, in vrx::WildlifeScoringPlugin::Implementation::PublishAnimalLocations(gz::sim::v7::UpdateInfo const&, gz::sim::v7::EntityComponentManager&)
[ruby $(which gz) sim-1] #1    Object "/home/mrmccarr@ern.nps.edu/gazebo_ws/install/lib/libgz-transport12.so.12", at 0x7fbc18924d77, in gz::transport::v12::Node::Publisher::Publish(google::protobuf::Message const&)
[ruby $(which gz) sim-1] #0    Object "/home/mrmccarr@ern.nps.edu/gazebo_ws/install/lib/libgz-transport12.so.12", at 0x7fbc189240cd, in gz::transport::v12::Node::Publisher::Valid() const
[ruby $(which gz) sim-1] Segmentation fault (Address not mapped to object [0x8])

World 1 still works fine...

caguero commented 1 year ago

Hmm, practice_2022_wildlife2_task works for me...

M1chaelM commented 1 year ago

Hmm, practice_2022_wildlife2_task works for me...

Ok thanks for checking. Maybe something is stale. I'll try with a fresh environment.

M1chaelM commented 1 year ago

No luck. Steps to reproduce:

  1. Open a new terminal.
  2. Build from scratch: 
    source /opt/ros/humble/setup.bash
cd ~/vrx_ws
rm -rf build install log
GZ_VERSION=garden colcon build --merge-install

  3. Open a second terminal

    cd ~/vrx_ws
. install/setup.bash

ros2 launch vrx_gz competition.launch.py world:=practice_2022_wildlife2_task
  4. Wait for the task to enter the OnRunning stage. It crashes.

https://user-images.githubusercontent.com/8611855/235728972-61e19f4b-ee3e-4c90-b949-92784a41d71f.mp4

caguero commented 1 year ago

No luck. Steps to reproduce:

  1. Open a new terminal.
  2. Build from scratch: 
    source /opt/ros/humble/setup.bash
cd ~/vrx_ws
rm -rf build install log
GZ_VERSION=garden colcon build --merge-install

  3. Open a second terminal

    cd ~/vrx_ws
. install/setup.bash

ros2 launch vrx_gz competition.launch.py world:=practice_2022_wildlife2_task

  4. Wait for the task to enter the OnRunning stage. It crashes.

    wildlife2_crash_2.mp4

I'm sorry, I didn't realize it was happening after the onRunning() event. It should be fixed now.