Extended GameLogicPlugin to check for vehicle flips #519
Conversation
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Signed-off-by: Ashton Larkin <[email protected]>
azeey
reviewed
Aug 3, 2020
azeey
reviewed
Aug 6, 2020
Signed-off-by: Ashton Larkin <[email protected]>
Signed-off-by: Ashton Larkin <[email protected]>
azeey
reviewed
Aug 6, 2020
subt_ign/src/GameLogicPlugin.cc
Outdated
| (std::chrono::seconds(s) + std::chrono::nanoseconds(ns)); | ||
|
|
||
| // check if the robot has flipped | ||
| this->dataPtr->CheckRobotFlip(name, pose); |
There was a problem hiding this comment.
Should we store whether the robot is already flipped? Otherwise, once it flips, the log file will be flooded with flip events. It might also be a good idea to record an event only after the robot stays flipped for a duration of time to avoid false positives.
There was a problem hiding this comment.
I added a data structure (map) that keeps track of the robot's flip information in 40f1186. I used this map to make sure a robot has been flipped for a few seconds before logging a flip event, and also used it to only log a flip event once per new flip.
subt_ign/src/GameLogicPlugin.cc
Outdated
| (std::chrono::seconds(s) + std::chrono::nanoseconds(ns)); | ||
|
|
||
| // check if the robot has flipped | ||
| this->dataPtr->CheckRobotFlip(name, pose); |
There was a problem hiding this comment.
For one PostUpdate, this runs once for every sensor in a robot. Can we limit it to run just once for a robot?
There was a problem hiding this comment.
I went ahead and moved the CheckRobotFlip call outside of the sensor portion in 1256865, so I believe it should only be called once per PostUpdate now.
Signed-off-by: Ashton Larkin <[email protected]>
Signed-off-by: Ashton Larkin <[email protected]>
nkoenig
approved these changes
Aug 7, 2020
nkoenig
added a commit
that referenced
this pull request
Aug 10, 2020
* add test links to specifications.md and modify parameters in spawn.rb * Model submission for the OZBOT ATR sensor configurations 1 and 2 from CSIRO_DATA61 * added thumbnail of vehicle * Model submission for Gagarin sensor configuration 1 from CERBERUS * fixed issues from PR feedback: drive plugin settings, gas sensor, joint state publisher, imu noise, lidar params and camera settings * add barometer to m100 * turning radius note and config2 power load fix * Edited README.md addressing comments Signed-off-by: Mihir Kulkarni <[email protected]> * Apply velocity and acceleration limits to submitted configs (#498) * Limit velocity and acceleration on costar husky Signed-off-by: Nate Koenig <[email protected]> * clarify limits in specifications.md * add limits to remaining submitted configs * remove diversion since data was provided Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Angela Maio <[email protected]> Co-authored-by: Angela Maio <[email protected]> * Restoring velocity controller for moving gimbals (#503) Signed-off-by: Carlos Aguero <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Changed plugin for Picoflexx from gpu_ray to depth_camera Signed-off-by: Mihir Kulkarni <[email protected]> * Fix spawner Signed-off-by: Nate Koenig <[email protected]> * Revert run Signed-off-by: Nate Koenig <[email protected]> * Fix z offset, and cleanup Signed-off-by: Nate Koenig <[email protected]> * Added marsupial capabilities Signed-off-by: Nate Koenig <[email protected]> * Added Cerberus Gagarin Signed-off-by: Nate Koenig <[email protected]> * Bridge gas topic Signed-off-by: Nate Koenig <[email protected]> * Added Gagarin to X1 and R2 Signed-off-by: Nate Koenig <[email protected]> * Ctu cras norlab absolem sensor config 1 2 (#528) * Added CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_1 and CTU_CRAS_NORLAB_ABSOLEM_SENSOR_CONFIG_2 (which only differ by breadcrumbs). * Added thumbnails. * Fixed realsense, omnicam, lights and mass properties. * Changed order of elements in joint_states to correspond to the real robot. * Added plugin for automatic handling of laser rotation. * Fixed problem with loading robot_description. * Fixed IMU parameters. * Fixed collision when flipped. * WIP: Flipper control plugin. * Added position control for remaining flippers Signed-off-by: Nate Koenig <[email protected]> * Fix realsense camera Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Martin Pecka <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Make Absolem turn Signed-off-by: Nate Koenig <[email protected]> * Download new models Signed-off-by: Nate Koenig <[email protected]> * Remove extra topics and joint states from Absolem model Signed-off-by: Nate Koenig <[email protected]> * match API for RGBD * Fix position control Signed-off-by: Nate Koenig <[email protected]> * Fix comment end Signed-off-by: Nate Koenig <[email protected]> * Moved and realigned RGBD camera to not be obstructed by realsense mesh (#530) * Moved and realigned RGBD camera to not be obstructed by realsense mesh. * Adjust RGBD camera resolution to 640x480 to match other D435 sensors in the repository. * add barometer to model.sdf * match topic names to API * Adjustment of battery plugin on the CSIRO Data61 OZBOT ATR model to use generic X1 parameters tuned to 60 minutes of battery life until validation data is provided. * Record marsupial pairs (#531) * Record marsupial pairs Signed-off-by: Nate Koenig <[email protected]> * Filter on performers Signed-off-by: Nate Koenig <[email protected]> * Cleanup Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Monitor for dead battery events (#533) Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Added a few missing optical frames, and removed the extra rgbd parameters from cerberus_m100 (#525) Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Rotate marsupials (#536) * rotate marsupial children backwards for R2 and OZBOT * Added rotations to other launch files Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Angela Maio <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Fix marsupial joints (#535) Signed-off-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> * Remove extra topics and joint states from Absolem model (#529) * Remove extra topics and joint states from Absolem model Signed-off-by: Nate Koenig <[email protected]> * match API for RGBD * Fix position control Signed-off-by: Nate Koenig <[email protected]> * Fix comment end Signed-off-by: Nate Koenig <[email protected]> * rename omni topics * Fixed and added flipper velocity capacity for raw commanded velocities. * match gimbal cmd to API * match joint_state to API Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Angela Maio <[email protected]> Co-authored-by: Arthur Schang <[email protected]> * Apply effort limits to the Absolem model Signed-off-by: Nate Koenig <[email protected]> * Fix horizondal typo Signed-off-by: Nate Koenig <[email protected]> * Fix sensor name Signed-off-by: Nate Koenig <[email protected]> * Proper tof_bottom rename Signed-off-by: Nate Koenig <[email protected]> * Trying effort of 40 Signed-off-by: Nate Koenig <[email protected]> * Trying a value of 30 Signed-off-by: Nate Koenig <[email protected]> * effort=20 allows some obstacle traversal but no tiptoe driving * Extended GameLogicPlugin to check for vehicle flips (#519) * check for vehicle flips in GameLogicPlugin Signed-off-by: Ashton Larkin <[email protected]> * using rotation matrix and dot product to check vehicle flip Signed-off-by: Ashton Larkin <[email protected]> * cleaned up math for checking vehicle flips Signed-off-by: Ashton Larkin <[email protected]> * moved check flip call outside of sensor portion in PostUpdate Signed-off-by: Ashton Larkin <[email protected]> * added timeout to flip detection that handles false positives Signed-off-by: Ashton Larkin <[email protected]> * Fix breadcrumb deployment location Signed-off-by: Nate Koenig <[email protected]> * update absolem specs after fixes and breadcrumbs * update ozbot specs with breadcrumbs * Adjustment of the FoV parameter to match resolution. (#539) Co-authored-by: Hong <[email protected]> Co-authored-by: Tom Molnar <[email protected]> Co-authored-by: Mihir Kulkarni <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Nate Koenig <[email protected]> Co-authored-by: Carlos Agüero <[email protected]> Co-authored-by: Martin Pecka <[email protected]> Co-authored-by: acschang <[email protected]> Co-authored-by: Arthur Schang <[email protected]> Co-authored-by: adlarkin <[email protected]>
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
I'm checking for vehicle flips by checking the angle between the world's z-axis and the vehicle's z-axis (if the angle is close to 180 degrees, then the robot is probably flipped).
Signed-off-by: Ashton Larkin [email protected]