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.

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

'Scan, dock and deliver' scoring plugin #333

Merged
merged 21 commits into from
Sep 9, 2021
Merged

Conversation

caguero
Copy link
Contributor

@caguero caguero commented Sep 6, 2021

See issue #278.

This pull request updates the scoring plugin associated to the Scan, dock and deliver task. It should fully match what we describe in the rules.

  • It fixes a few issues with the docking. The activation areas should work now.
  • It updates the 2022 docks including their activation areas.
    • There's one external activation area and one internal activation area per gate.
    • There are two activation areas for the shooting targets per placard.
  • It updates the points awarded for docking (now 15 and 20 points are awarded for a docking event and a successful docking event respectively).
  • It grants new points when the targets are hit with the ball shooter.
  • It updates the ball shooter (angle and force applied when firing).

For testing:

roslaunch vrx_gazebo scan_dock_deliver.launch extra_gazebo_args:=--verbose

You can teleoperate the WAM-V with:

roslaunch vrx_gazebo usv_joydrive.launch

For checking the score:

rostopic echo /vrx/task/info

Try reporting the color sequence:

rosservice call /vrx/scan_dock_deliver/color_sequence "blue" "green" "red"

Try teleoperating the WAM-V to the dock. You should observe the following sequence of debug messages:

[Msg] Entering external dock activation zone in [bay2]
[Dbg] [scan_dock_scoring_plugin.cc:271] [bay2] OnExternalActivationEvent(): 1
[Msg] Leaving external dock activation zone in [bay2]
[Dbg] [scan_dock_scoring_plugin.cc:271] [bay2] OnExternalActivationEvent(): 0
[Msg] Entering internal dock activation zone, transitioning to <docking> state in [bay2].
[Dbg] [scan_dock_scoring_plugin.cc:251] [bay2] OnInternalActivationEvent(): 1
[Msg] Successfully stayed in dock for 10 seconds, transitioning to <docked> state
[Msg] Leaving internal dock activation zone in [bay2] after required time - transitioning to <exited> state.
[Dbg] [scan_dock_scoring_plugin.cc:251] [bay2] OnInternalActivationEvent(): 0
[Msg] Entering external dock activation zone in [bay2]
[Dbg] [scan_dock_scoring_plugin.cc:271] [bay2] OnExternalActivationEvent(): 1
[Msg] Successfully docked in [bay2]. Awarding 15 points.
[Msg] Docked in correct dock [bay2]. Awarding 20 more points.

Note that docking and exiting in the right bay will finish the task.

Try shooting some of the targets:

gz model -m wamv -x 555.21 -y -229.75 -z 0 -Y 0
rostopic pub /wamv/shooters/ball_shooter/fire std_msgs/Empty "{}" --once

Check scan_dock_deliver.world.xacro for gz model commands to teleport the robot to the right spot for testing the ball shooting.

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
@caguero caguero requested a review from M1chaelM September 6, 2021 22:04
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
@caguero caguero changed the base branch from caguero/dock_2022 to master September 7, 2021 15:39
Copy link
Collaborator

@M1chaelM M1chaelM left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This looks like it's almost complete. Everything worked except as noted below:

color sequence

To submit the color sequence, I had to use a command that varies slightly from what is given in the instructions:

rosservice call /vrx/scan_dock_deliver/color_sequence "blue" "green" "red"

Score for docking events

The scoring for docking is not what I intended. Looking at the documentation I can see this is ambiguous and needs to be clarified. The intention was that the teams would receive 20 points (total) if they dock in the correct bay, but if they dock in the wrong bay they still get 15 points. In other words, you get 5 extra points for getting it right. This matches what RobotX scoring guidance is recommending. Right now our implementation gives teams an additional 20 points for docking in the correct bay (so 35 points total). I will clarify this in the documentation, and I think we should fix the scoring so it's only a 5 point bonus.

Score for ball shooter

The coordinates given in the testing instructions will place the WAMV in a location where it can successfully fire a ball into the large hole of the placard marked with a red circle. To test the small hole, you can use:

gz model -m wamv -x 555.3 -y -230 -z 0 -Y -0.2

These seem to be working correctly with the exception that it is currently possible to fire as many projectiles as you want and get points for all of them. We say in the documentation that the number will be limited to 4. I think this is just a matter of setting the num_shots parameter to 4 (though this will also affect the ball shooter in general).

@@ -45,7 +45,7 @@
<frame>wamv/ball_shooter_link</frame>
<pose>0.14 0 0.09 0 0 0</pose>
</projectile>
<shot_force>250</shot_force>
<shot_force>300</shot_force>
<topic>${namespace}/${shooter_namespace}${name}/fire</topic>
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

As mentioned in general comments I think we also need

<num_shots> 4</num_shots>

to align with our documentation for this task.

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
@caguero
Copy link
Contributor Author

caguero commented Sep 8, 2021

This looks like it's almost complete. Everything worked except as noted below:

color sequence

To submit the color sequence, I had to use a command that varies slightly from what is given in the instructions:

rosservice call /vrx/scan_dock_deliver/color_sequence "blue" "green" "red"

I just hit tab to autocomplete and it included the field names and the single quotes. Probably both options are valid.

Score for docking events

The scoring for docking is not what I intended. Looking at the documentation I can see this is ambiguous and needs to be clarified. The intention was that the teams would receive 20 points (total) if they dock in the correct bay, but if they dock in the wrong bay they still get 15 points. In other words, you get 5 extra points for getting it right. This matches what RobotX scoring guidance is recommending. Right now our implementation gives teams an additional 20 points for docking in the correct bay (so 35 points total). I will clarify this in the documentation, and I think we should fix the scoring so it's only a 5 point bonus.

Thanks for the clarification! I updated the bonus in the code. See c8285ce.

Score for ball shooter

The coordinates given in the testing instructions will place the WAMV in a location where it can successfully fire a ball into the large hole of the placard marked with a red circle. To test the small hole, you can use:

gz model -m wamv -x 555.3 -y -230 -z 0 -Y -0.2

These seem to be working correctly with the exception that it is currently possible to fire as many projectiles as you want and get points for all of them. We say in the documentation that the number will be limited to 4. I think this is just a matter of setting the num_shots parameter to 4 (though this will also affect the ball shooter in general).

I'm aware of this and I added a note to this task's wiki clarifying it. The problem is that the number of shots is a global parameter, not a per-task parameter. I'm leaning towards leaving as it is for letting the teams practice with the understanding that we'll limit it during the competition.

@M1chaelM
Copy link
Collaborator

M1chaelM commented Sep 8, 2021

This looks like it's almost complete. Everything worked except as noted below:

color sequence

To submit the color sequence, I had to use a command that varies slightly from what is given in the instructions:

rosservice call /vrx/scan_dock_deliver/color_sequence "blue" "green" "red"

I just hit tab to autocomplete and it included the field names and the single quotes. Probably both options are valid.

When I use the one you provided, the service crashes with the following error:

rosservice call /vrx/scan_dock_deliver/color_sequence "color1: 'blue' color2: 'green' color3: 'red'"
Traceback (most recent call last):
  File "/opt/ros/noetic/bin/rosservice", line 35, in <module>
    rosservice.rosservicemain()
  File "/opt/ros/noetic/lib/python3/dist-packages/rosservice/__init__.py", line 749, in rosservicemain
    _rosservice_cmd_call(argv)
  File "/opt/ros/noetic/lib/python3/dist-packages/rosservice/__init__.py", line 610, in _rosservice_cmd_call
    service_args.append(yaml.safe_load(arg))
  File "/usr/lib/python3/dist-packages/yaml/__init__.py", line 162, in safe_load
    return load(stream, SafeLoader)
  File "/usr/lib/python3/dist-packages/yaml/__init__.py", line 114, in load
    return loader.get_single_data()
  File "/usr/lib/python3/dist-packages/yaml/constructor.py", line 49, in get_single_data
    node = self.get_single_node()
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 36, in get_single_node
    document = self.compose_document()
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 55, in compose_document
    node = self.compose_node(None, None)
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 84, in compose_node
    node = self.compose_mapping_node(anchor)
  File "/usr/lib/python3/dist-packages/yaml/composer.py", line 127, in compose_mapping_node
    while not self.check_event(MappingEndEvent):
  File "/usr/lib/python3/dist-packages/yaml/parser.py", line 98, in check_event
    self.current_event = self.state()
  File "/usr/lib/python3/dist-packages/yaml/parser.py", line 438, in parse_block_mapping_key
    raise ParserError("while parsing a block mapping", self.marks[-1],
yaml.parser.ParserError: while parsing a block mapping
  in "<unicode string>", line 1, column 1:
    color1: 'blue' color2: 'green' c ... 
    ^
expected <block end>, but found '<scalar>'
  in "<unicode string>", line 1, column 16:
    color1: 'blue' color2: 'green' color3: 'red'
                   ^

I haven't looked into it but I wonder if this is another problem caused by the switch to Python3.

Score for ball shooter

These seem to be working correctly with the exception that it is currently possible to fire as many projectiles as you want and get points for all of them. We say in the documentation that the number will be limited to 4. I think this is just a matter of setting the num_shots parameter to 4 (though this will also affect the ball shooter in general).

I'm aware of this and I added a note to this task's wiki clarifying it. The problem is that the number of shots is a global parameter, not a per-task parameter. I'm leaning towards leaving as it is for letting the teams practice with the understanding that we'll limit it during the competition.

OK, I think there are still two things we can do that would further prevent confusion, if you agree:

  1. Set the default to 4 shots and put a note in the wiki telling teams how to turn it off for purposes of practice, instead of the other way around. I think this is a little better as a default because even teams that don't read the documentation carefully will still have an experience that's close to the real event.
  2. Modify the scoring plugin so it refuses to score more than 4 shots. This means the teams can still "cheat" by shooting lots of times, but at least they can't end up with scores higher than the maximum.

What do you think?

@M1chaelM
Copy link
Collaborator

M1chaelM commented Sep 8, 2021

Another note regarding the ros service argument: I tried using the autocomplete feature to reproduce the argument you mentioned and I got this one with line breaks:

rosservice call /vrx/scan_dock_deliver/color_sequence "color1: ''
color2: ''
color3: ''" 

This argument seems to work fine. Without the line breaks I get the crash listed above. I'm not sure why or if there is an easy way to make the service a little more forgiving.

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
@caguero
Copy link
Contributor Author

caguero commented Sep 9, 2021

What do you think?

Sounds good! I limited the shots to 4 and documented in the wiki how to set unlimited shots for practice. See c8363ae.

As for the service request from the command line I was also able to reproduce your issue. I didn't realize that without autocompleting, the line breaks matter. I think to fix this is out of scope. Your suggestion for calling it via command line is totally fine (I updated the wiki). During the competition the teams will call this service programmatically.

@M1chaelM
Copy link
Collaborator

M1chaelM commented Sep 9, 2021

What do you think?

Sounds good! I limited the shots to 4 and documented in the wiki how to set unlimited shots for practice. See c8363ae.

As for the service request from the command line I was also able to reproduce your issue. I didn't realize that without autocompleting, the line breaks matter. I think to fix this is out of scope. Your suggestion for calling it via command line is totally fine (I updated the wiki). During the competition the teams will call this service programmatically.

Great! I tested again and I'm getting the expected 4-shot limit, so I think that covers everything. I'll approve in a second.

Copy link
Collaborator

@M1chaelM M1chaelM left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

All looks good. Thanks!

Signed-off-by: Carlos Agüero <caguero@openrobotics.org>
@caguero caguero merged commit a7a2d26 into master Sep 9, 2021
@caguero caguero deleted the caguero/deliver_scoring branch January 5, 2023 21:25
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

2 participants