├── NEWS ├── AUTHORS ├── msg ├── Leak.msg ├── Console.msg ├── Harness.msg ├── Score.msg ├── Satellite.msg └── Task.msg ├── .hgignore ├── cmake ├── 99_srcsim_setup.sh.in └── setup.bash.in ├── models ├── mars_crate │ ├── materials │ │ └── textures │ │ │ └── crate.png │ ├── model.config │ ├── meshes │ │ └── crate.mtl │ └── model.sdf ├── src_habitat │ ├── materials │ │ ├── textures │ │ │ ├── finish.png │ │ │ ├── hdu_01.png │ │ │ ├── hdu_02.png │ │ │ └── Metal_Seamed.png │ │ └── scripts │ │ │ └── habitat.material │ ├── model.config │ ├── meshes │ │ └── hdu.mtl │ └── model.sdf ├── mars_crate_long │ ├── materials │ │ └── textures │ │ │ └── crate.png │ ├── meshes │ │ └── crate.mtl │ ├── model.config │ └── model.sdf ├── mars_rock_1 │ ├── materials │ │ └── textures │ │ │ └── mars_rocks.png │ ├── model.config │ ├── meshes │ │ └── mars_rocks.mtl │ └── model.sdf ├── mars_rock_2 │ ├── materials │ │ └── textures │ │ │ └── mars_rocks.png │ ├── model.config │ ├── meshes │ │ └── mars_rocks.mtl │ └── model.sdf ├── mars_rock_3 │ ├── materials │ │ └── textures │ │ │ └── mars_rocks.png │ ├── model.config │ ├── meshes │ │ └── mars_rocks.mtl │ └── model.sdf ├── solar_array │ ├── materials │ │ └── textures │ │ │ └── solar_array.png │ ├── model.config │ ├── meshes │ │ └── solar_array.mtl │ └── model.sdf ├── solar_panel │ ├── materials │ │ └── textures │ │ │ └── solar_panel.png │ ├── model.config │ └── meshes │ │ └── solar_panel.mtl ├── space_table │ ├── materials │ │ └── textures │ │ │ └── space_table.png │ ├── model.config │ └── meshes │ │ └── space_table.mtl ├── src_doorway_q2 │ ├── materials │ │ ├── textures │ │ │ ├── Doorway.png │ │ │ └── button.png │ │ └── scripts │ │ │ └── button.material │ └── model.config ├── heightmap_mars │ ├── materials │ │ └── textures │ │ │ ├── mars_tex1.jpg │ │ │ ├── mars_tex2.jpg │ │ │ ├── mars_tex3.jpg │ │ │ └── marssmall.jpg │ ├── model.config │ └── model.sdf ├── satellite_dish │ ├── materials │ │ └── textures │ │ │ └── satellite.png │ ├── model.config │ └── meshes │ │ └── satellite_dish.mtl ├── mars_explorer │ ├── materials │ │ └── textures │ │ │ ├── mars_explorer.png │ │ │ └── mars_explorer_trailer.png │ ├── model.config │ └── meshes │ │ └── mars_explorer.mtl ├── leak_patch_tool │ ├── materials │ │ └── textures │ │ │ └── leak_patch_tool.png │ ├── model.config │ ├── meshes │ │ └── leak_patch_tool.mtl │ └── model.sdf ├── air_leak_detector │ ├── materials │ │ └── textures │ │ │ └── air_leak_detector.png │ ├── model.config │ ├── meshes │ │ └── air_leak_detector.mtl │ └── model.sdf ├── src_walkway_metal_straight │ ├── materials │ │ └── textures │ │ │ └── walkway.png │ ├── meshes │ │ ├── mesh.mtl │ │ └── mesh.obj │ ├── model.config │ └── model.sdf ├── walkway_finish_adjacent │ ├── materials │ │ └── textures │ │ │ └── walkway_finish.png │ ├── model.config │ ├── meshes │ │ └── walkway_finish_adjacent.mtl │ └── model.sdf ├── walkway_finish_opposite │ ├── materials │ │ └── textures │ │ │ └── walkway_finish.png │ ├── model.config │ ├── meshes │ │ └── walkway_finish_opposite.mtl │ └── model.sdf ├── src_walkway_metal_45 │ ├── meshes │ │ ├── mesh.mtl │ │ └── mesh.obj │ ├── model.config │ └── model.sdf └── src_walkway_metal_90 │ ├── meshes │ ├── mesh.mtl │ └── mesh.obj │ ├── model.config │ └── model.sdf ├── launch ├── grasping_init_wait.launch ├── grasping_init.launch ├── multisense.launch ├── qual1.launch ├── unique.launch ├── qual2.launch ├── unique_task1.launch ├── unique_task2.launch ├── unique_task3.launch └── finals.launch ├── srv └── StartTask.srv ├── scripts ├── wait_for_controller.sh ├── test_cheat_detect.py ├── rossleep.py ├── init_robot.sh ├── neck_traj_yaml.py ├── cheat_detect.py ├── head_traj_yaml.py ├── arm_traj_yaml.py ├── Traj_data.yaml └── walk_test.py ├── README.md ├── LICENSE ├── config └── val_grasping.yaml ├── package.xml ├── worlds ├── leak.erb ├── qual2.world └── air_leak_detector.erb ├── include └── srcsim │ ├── Qual2Plugin.hh │ ├── FinalsPlugin.hh │ ├── srcsim_ros_harness.h │ ├── SolarPanelPlugin.hh │ ├── SatellitePlugin.hh │ ├── BoxContainsPlugin.hh │ ├── Task1.hh │ ├── Qual1Plugin.hh │ ├── SRCHarnessPlugin.hh │ ├── HarnessManager.hh │ ├── Checkpoint.hh │ ├── Task2.hh │ ├── Task.hh │ └── SRCMultiSenseSLPlugin.hh ├── scoring ├── scoring_q2.rb └── common.rb └── src ├── Qual2Plugin.cc ├── BoxContainsPlugin.cc ├── srcsim_ros_harness.cpp └── Task1.cc /NEWS: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------------------- /AUTHORS: -------------------------------------------------------------------------------- 1 | OSRFoundation 2 | -------------------------------------------------------------------------------- /msg/Leak.msg: -------------------------------------------------------------------------------- 1 | # 0 to 1 2 | float64 value 3 | 4 | -------------------------------------------------------------------------------- /.hgignore: -------------------------------------------------------------------------------- 1 | syntax: glob 2 | 3 | build 4 | build_* 5 | .DS_Store 6 | *.swp 7 | -------------------------------------------------------------------------------- /msg/Console.msg: -------------------------------------------------------------------------------- 1 | float64 x 2 | float64 y 3 | float64 z 4 | float64 r 5 | float64 g 6 | float64 b 7 | -------------------------------------------------------------------------------- /cmake/99_srcsim_setup.sh.in: -------------------------------------------------------------------------------- 1 | export GAZEBO_MODEL_PATH=@CMAKE_INSTALL_PREFIX@/share/@PROJECT_NAME@/models:$GAZEBO_MODEL_PATH 2 | -------------------------------------------------------------------------------- /models/mars_crate/materials/textures/crate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/mars_crate/materials/textures/crate.png -------------------------------------------------------------------------------- /models/src_habitat/materials/textures/finish.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/src_habitat/materials/textures/finish.png -------------------------------------------------------------------------------- /models/src_habitat/materials/textures/hdu_01.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/src_habitat/materials/textures/hdu_01.png -------------------------------------------------------------------------------- /models/src_habitat/materials/textures/hdu_02.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/src_habitat/materials/textures/hdu_02.png -------------------------------------------------------------------------------- /models/mars_crate_long/materials/textures/crate.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/mars_crate_long/materials/textures/crate.png -------------------------------------------------------------------------------- /models/mars_rock_1/materials/textures/mars_rocks.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/mars_rock_1/materials/textures/mars_rocks.png -------------------------------------------------------------------------------- /models/mars_rock_2/materials/textures/mars_rocks.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/mars_rock_2/materials/textures/mars_rocks.png -------------------------------------------------------------------------------- /models/mars_rock_3/materials/textures/mars_rocks.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/mars_rock_3/materials/textures/mars_rocks.png -------------------------------------------------------------------------------- /models/solar_array/materials/textures/solar_array.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/solar_array/materials/textures/solar_array.png -------------------------------------------------------------------------------- /models/solar_panel/materials/textures/solar_panel.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/solar_panel/materials/textures/solar_panel.png -------------------------------------------------------------------------------- /models/space_table/materials/textures/space_table.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/space_table/materials/textures/space_table.png -------------------------------------------------------------------------------- /models/src_doorway_q2/materials/textures/Doorway.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/src_doorway_q2/materials/textures/Doorway.png -------------------------------------------------------------------------------- /models/src_doorway_q2/materials/textures/button.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/src_doorway_q2/materials/textures/button.png -------------------------------------------------------------------------------- /models/heightmap_mars/materials/textures/mars_tex1.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/heightmap_mars/materials/textures/mars_tex1.jpg -------------------------------------------------------------------------------- /models/heightmap_mars/materials/textures/mars_tex2.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/heightmap_mars/materials/textures/mars_tex2.jpg -------------------------------------------------------------------------------- /models/heightmap_mars/materials/textures/mars_tex3.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/heightmap_mars/materials/textures/mars_tex3.jpg -------------------------------------------------------------------------------- /models/heightmap_mars/materials/textures/marssmall.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/heightmap_mars/materials/textures/marssmall.jpg -------------------------------------------------------------------------------- /models/satellite_dish/materials/textures/satellite.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/satellite_dish/materials/textures/satellite.png -------------------------------------------------------------------------------- /models/src_habitat/materials/textures/Metal_Seamed.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/src_habitat/materials/textures/Metal_Seamed.png -------------------------------------------------------------------------------- /launch/grasping_init_wait.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /models/mars_explorer/materials/textures/mars_explorer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/mars_explorer/materials/textures/mars_explorer.png -------------------------------------------------------------------------------- /models/leak_patch_tool/materials/textures/leak_patch_tool.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/leak_patch_tool/materials/textures/leak_patch_tool.png -------------------------------------------------------------------------------- /models/air_leak_detector/materials/textures/air_leak_detector.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/air_leak_detector/materials/textures/air_leak_detector.png -------------------------------------------------------------------------------- /models/mars_explorer/materials/textures/mars_explorer_trailer.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/mars_explorer/materials/textures/mars_explorer_trailer.png -------------------------------------------------------------------------------- /models/src_walkway_metal_straight/materials/textures/walkway.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/src_walkway_metal_straight/materials/textures/walkway.png -------------------------------------------------------------------------------- /models/walkway_finish_adjacent/materials/textures/walkway_finish.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/walkway_finish_adjacent/materials/textures/walkway_finish.png -------------------------------------------------------------------------------- /models/walkway_finish_opposite/materials/textures/walkway_finish.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/osrf/srcsim/HEAD/models/walkway_finish_opposite/materials/textures/walkway_finish.png -------------------------------------------------------------------------------- /models/src_habitat/materials/scripts/habitat.material: -------------------------------------------------------------------------------- 1 | material FinishBox 2 | { 3 | technique 4 | { 5 | pass 6 | { 7 | texture_unit 8 | { 9 | texture finish.png 10 | } 11 | } 12 | } 13 | } 14 | 15 | -------------------------------------------------------------------------------- /srv/StartTask.srv: -------------------------------------------------------------------------------- 1 | # ID of task to start (1, 2 or 3) 2 | uint8 task_id 3 | 4 | # ID of checkpoint to start within that task. Depending on the task, this may 5 | # go from 1 to 8. 6 | uint8 checkpoint_id 7 | 8 | --- 9 | 10 | # True if checkpoint started successfully 11 | bool success 12 | -------------------------------------------------------------------------------- /scripts/wait_for_controller.sh: -------------------------------------------------------------------------------- 1 | #!/bin/sh 2 | 3 | while [ -z "$(rosparam list | grep ihmc_valkyrie_control_java_bridge)" ]; do 4 | echo "waiting for ihmc_valkyrie_control_java_bridge" 5 | sleep 1 6 | done 7 | 8 | echo "Found ihmc_valkyrie_control_java_bridge! Launching grasp_init." 9 | 10 | sleep 5 11 | 12 | roslaunch srcsim grasping_init.launch 13 | -------------------------------------------------------------------------------- /models/src_doorway_q2/materials/scripts/button.material: -------------------------------------------------------------------------------- 1 | material src_doorway_q2_button/Diffuse 2 | { 3 | receive_shadows off 4 | technique 5 | { 6 | pass 7 | { 8 | texture_unit 9 | { 10 | texture button.png 11 | filtering anistropic 12 | max_anisotropy 16 13 | } 14 | 15 | } 16 | } 17 | } 18 | 19 | -------------------------------------------------------------------------------- /models/heightmap_mars/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Heightmap Mars 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Mars heightmap 15 | 16 | 17 | -------------------------------------------------------------------------------- /models/src_doorway_q2/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | SRC: Doorway 5 | 0.1.0 6 | model.sdf 7 | 8 | Carlos Agüero 9 | caguero@osrfoundation.org 10 | 11 | 12 | A doorway used in the Space Robotics Challenge. 13 | 14 | 15 | -------------------------------------------------------------------------------- /scripts/test_cheat_detect.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | from gazebo_msgs.msg import LinkStates 5 | from gazebo_msgs.msg import LinkState 6 | 7 | def callback(msg): 8 | rospy.logerr("I am a cheater") 9 | 10 | rospy.init_node("CheatTest") 11 | 12 | pub = rospy.Publisher("/gazebo/set_link_state", LinkState) 13 | sub = rospy.Subscriber("/gazebo/link_states", LinkStates, callback) 14 | 15 | rospy.spin() 16 | -------------------------------------------------------------------------------- /scripts/rossleep.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import rospy 3 | import sys 4 | 5 | if len(sys.argv) != 2: 6 | print("Proper usage: %s " % sys.argv[0]) 7 | sys.exit(1) 8 | 9 | try: 10 | seconds = float(sys.argv[1]) 11 | except (ValueError): 12 | print("Couldn't convert argument [%s] to float" % sys.argv[1]) 13 | sys.exit(1) 14 | 15 | rospy.init_node('sleeper', anonymous=True) 16 | rospy.sleep(rospy.Duration.from_sec(seconds)) 17 | -------------------------------------------------------------------------------- /models/solar_array/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Solar Array 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A Martian solar array 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/solar_panel/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Solar Panel 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A Martian solar panel 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/src_walkway_metal_straight/meshes/mesh.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 30.11.2016 17:16:45 3 | 4 | newmtl Walkway 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka walkway.png 16 | map_Kd walkway.png 17 | -------------------------------------------------------------------------------- /models/mars_crate/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Mars Crate 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A crate you'd bring to Mars, the planet. 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/mars_explorer/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Mars Explorer 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | Something you'd use to explore Mars 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/space_table/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Space table 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | One of those tables people use in space 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/mars_rock_1/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Mars Rock 1 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A rock you'd see when walking around on Mars 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/leak_patch_tool/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Leak Patch Tool 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | SRC Finals task 3: a tool to patch air leaks 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/mars_rock_2/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Mars Rock 2 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | Another rock you'd see when walking around on Mars 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/air_leak_detector/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Air Leak Detector 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | SRC Finals task 3: a tool to detect air leaks 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/mars_rock_3/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Mars Rock 3 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | Yet another rock you'd see when walking around on Mars 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/satellite_dish/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Satellite Dish 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A satellite dish for communication to be installed on Mars. 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/mars_crate/meshes/crate.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 11.12.2016 14:24:47 3 | 4 | newmtl crate 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://mars_crate/materials/textures/crate.png 16 | map_Kd model://mars_crate/materials/textures/crate.png 17 | -------------------------------------------------------------------------------- /models/mars_crate_long/meshes/crate.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 11.12.2016 14:24:47 3 | 4 | newmtl crate 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://mars_crate/materials/textures/crate.png 16 | map_Kd model://mars_crate/materials/textures/crate.png 17 | -------------------------------------------------------------------------------- /models/mars_crate_long/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Mars Crate (long) 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A crate you'd bring to Mars, the planet, when you have lots of things. 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/walkway_finish_adjacent/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Walkway Finish Adjacent 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | Finish box which connects to 2 walkways on adjacent sides. 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/walkway_finish_opposite/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Walkway Finish Opposite 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Louise Poubel 10 | louise@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | Finish box which connects to 2 walkways on opposite sides. 19 | 20 | 21 | -------------------------------------------------------------------------------- /models/mars_rock_1/meshes/mars_rocks.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 11.12.2016 15:06:57 3 | 4 | newmtl mars_rocks 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://mars_rock_1/materials/textures/mars_rocks.png 16 | map_Kd model://mars_rock_1/materials/textures/mars_rocks.png 17 | -------------------------------------------------------------------------------- /models/mars_rock_2/meshes/mars_rocks.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 11.12.2016 15:06:57 3 | 4 | newmtl mars_rocks 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://mars_rock_1/materials/textures/mars_rocks.png 16 | map_Kd model://mars_rock_1/materials/textures/mars_rocks.png 17 | -------------------------------------------------------------------------------- /models/mars_rock_3/meshes/mars_rocks.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 11.12.2016 15:06:57 3 | 4 | newmtl mars_rocks 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://mars_rock_1/materials/textures/mars_rocks.png 16 | map_Kd model://mars_rock_1/materials/textures/mars_rocks.png 17 | -------------------------------------------------------------------------------- /models/solar_array/meshes/solar_array.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 09.03.2017 22:08:54 3 | 4 | newmtl solar_array 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://solar_array/materials/textures/solar_array.png 16 | map_Kd model://solar_array/materials/textures/solar_array.png 17 | -------------------------------------------------------------------------------- /models/solar_panel/meshes/solar_panel.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 07.02.2017 19:09:14 3 | 4 | newmtl solar_panel 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://solar_panel/materials/textures/solar_panel.png 16 | map_Kd model://solar_panel/materials/textures/solar_panel.png 17 | -------------------------------------------------------------------------------- /models/space_table/meshes/space_table.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 01.02.2017 18:28:56 3 | 4 | newmtl space_table 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://space_table/materials/textures/space_table.png 16 | map_Kd model://space_table/materials/textures/space_table.png 17 | -------------------------------------------------------------------------------- /models/satellite_dish/meshes/satellite_dish.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 06.04.2017 20:25:35 3 | 4 | newmtl Satellite 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://satellite_dish/materials/textures/satellite.png 16 | map_Kd model://satellite_dish/materials/textures/satellite.png 17 | -------------------------------------------------------------------------------- /models/src_habitat/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | SRC: Habitat 5 | 0.1.0 6 | model.sdf 7 | 8 | Carlos Agüero 9 | caguero@osrfoundation.org 10 | 11 | 12 | Louise Poubel 13 | louise@osrfoundation.org 14 | 15 | 16 | The Habitat Demonstration Unit (HDU) used in the Space Robotics Challenge. 17 | 18 | 19 | -------------------------------------------------------------------------------- /models/src_walkway_metal_45/meshes/mesh.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 30.11.2016 17:17:21 3 | 4 | newmtl Walkway 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://walkway_metal_straight/materials/textures/walkway.png 16 | map_Kd model://walkway_metal_straight/materials/textures/walkway.png 17 | -------------------------------------------------------------------------------- /models/src_walkway_metal_90/meshes/mesh.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 30.11.2016 17:17:52 3 | 4 | newmtl Walkway 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://walkway_metal_straight/materials/textures/walkway.png 16 | map_Kd model://walkway_metal_straight/materials/textures/walkway.png 17 | -------------------------------------------------------------------------------- /models/leak_patch_tool/meshes/leak_patch_tool.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 13.03.2017 20:17:23 3 | 4 | newmtl leak_patch_tool 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://leak_patch_tool/materials/textures/leak_patch_tool.png 16 | map_Kd model://leak_patch_tool/materials/textures/leak_patch_tool.png 17 | -------------------------------------------------------------------------------- /models/src_walkway_metal_straight/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | SRC Walkway Metal Straight 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | A straight metal walkway section. This model can be stitched together to create longer paths. 19 | SRC version 20 | 21 | 22 | -------------------------------------------------------------------------------- /launch/grasping_init.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /models/air_leak_detector/meshes/air_leak_detector.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 31.01.2017 19:42:26 3 | 4 | newmtl air_leak_detector 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://air_leak_detector/materials/textures/air_leak_detector.png 16 | map_Kd model://air_leak_detector/materials/textures/air_leak_detector.png 17 | -------------------------------------------------------------------------------- /models/walkway_finish_adjacent/meshes/walkway_finish_adjacent.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 31.01.2017 21:31:49 3 | 4 | newmtl walkway_finish 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://walkway_finish_adjacent/materials/textures/walkway_finish.png 16 | map_Kd model://walkway_finish_adjacent/materials/textures/walkway_finish.png 17 | -------------------------------------------------------------------------------- /models/walkway_finish_opposite/meshes/walkway_finish_opposite.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 31.01.2017 21:32:20 3 | 4 | newmtl walkway_finish 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://walkway_finish_opposite/materials/textures/walkway_finish.png 16 | map_Kd model://walkway_finish_opposite/materials/textures/walkway_finish.png 17 | -------------------------------------------------------------------------------- /msg/Harness.msg: -------------------------------------------------------------------------------- 1 | # "Enum" for harness states 2 | 3 | # Harness is detached 4 | uint8 NONE = 0 5 | 6 | # Harness is detached, soon will attach 7 | uint8 DETACH_TO_ATTACH = 1 8 | 9 | # Harness is attached, soon will lower 10 | uint8 ATTACH_TO_LOWER = 2 11 | 12 | # Harness is lowering, soon will change to high level control 13 | uint8 LOWER_TO_STAND = 3 14 | 15 | # Robot is changing to high level control, harness will detach soon 16 | uint8 STAND_TO_DETACH = 4 17 | 18 | # Harness is detaching, robot will be released soon 19 | uint8 DETACH_TO_NONE = 5 20 | 21 | # Current harness status 22 | uint8 status 23 | 24 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Space Robotics Challenge Simulation 2 | 3 | SRCSim is the software used by teams participating in the Space Robotics 4 | Challenge competition hosted by the Space Technology Mission Directorate (STMD) 5 | and NASA. 6 | 7 | ## Install 8 | 9 | Refer the [system setup tutorial](https://bitbucket.org/osrf/srcsim/wiki/system_setup). 10 | 11 | ## Resources 12 | 13 | 1. [Tutorials](https://bitbucket.org/osrf/srcsim/wiki/tutorials) 14 | 1. [Documentation](https://bitbucket.org/osrf/srcsim/wiki/documentation) 15 | 1. [Space Robotics Challenge website](https://www.nasa.gov/directorates/spacetech/centennial_challenges/space_robotics/index.html) 16 | 17 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (Apache License) 2 | 3 | Copyright 2012 Open Source Robotics Foundation 4 | 5 | Licensed under the Apache License, Version 2.0 (the "License"); 6 | you may not use this file except in compliance with the License. 7 | You may obtain a copy of the License at 8 | 9 | http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | Unless required by applicable law or agreed to in writing, software 12 | distributed under the License is distributed on an "AS IS" BASIS, 13 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | See the License for the specific language governing permissions and 15 | limitations under the License. 16 | -------------------------------------------------------------------------------- /models/mars_crate/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 0.36 0 0 0 8 | 9 | 10 | 0.72 0.72 0.72 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://mars_crate/meshes/crate.obj 18 | 0.03 0.03 0.03 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /msg/Score.msg: -------------------------------------------------------------------------------- 1 | # Array with the total time taken to complete each checkpoint across all tasks. 2 | # The time is zero for skipped checkpoints or checkpoints which were never 3 | # started due to task timeout. 4 | duration[] checkpoint_durations 5 | 6 | # Array with the total penalty time for each checkpoint across all tasks. 7 | duration[] checkpoint_penalties 8 | 9 | # The sum of the score across all checkpoints 10 | uint8 score 11 | 12 | # The sum of the time taken to complete all successful checkpoints, including 13 | # the time penalties for those checkpoints. 14 | # Incomplete checkpoints are not taken into account (neither the time spent 15 | # trying, nor the penalties) 16 | duration total_completion_time 17 | -------------------------------------------------------------------------------- /models/mars_crate_long/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 0.36 0 0 0 8 | 9 | 10 | 1.08 0.72 0.72 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://mars_crate_long/meshes/crate_long.obj 18 | 0.03 0.03 0.03 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /msg/Satellite.msg: -------------------------------------------------------------------------------- 1 | # Target pitch angle in radians 2 | float64 target_pitch 3 | 4 | # Target yaw angle in radians 5 | float64 target_yaw 6 | 7 | # Current pitch angle in radians 8 | float64 current_pitch 9 | 10 | # Current yaw angle in radians 11 | float64 current_yaw 12 | 13 | # True if the current pitch angle is within a tolerance distance from the 14 | # target pitch angle 15 | bool pitch_correct_now 16 | 17 | # True if the current yaw angle is within a tolerance distance from the 18 | # target yaw angle 19 | bool yaw_correct_now 20 | 21 | # True if the pitch angle has been correct for enough time 22 | bool pitch_completed 23 | 24 | # True if the yaw angle has been correct for enough time 25 | bool yaw_completed 26 | -------------------------------------------------------------------------------- /models/mars_rock_1/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://mars_rock_1/meshes/mars_rock_1.obj 10 | 0.03 0.03 0.03 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://mars_rock_1/meshes/mars_rock_1.obj 18 | 0.03 0.03 0.03 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /models/mars_rock_2/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://mars_rock_2/meshes/mars_rock_2.obj 10 | 0.03 0.03 0.03 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://mars_rock_2/meshes/mars_rock_2.obj 18 | 0.03 0.03 0.03 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /models/mars_rock_3/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://mars_rock_3/meshes/mars_rock_3.obj 10 | 0.03 0.03 0.03 11 | 12 | 13 | 14 | 15 | 16 | 17 | model://mars_rock_3/meshes/mars_rock_3.obj 18 | 0.03 0.03 0.03 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /models/src_walkway_metal_90/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | SRC Walkway Metal 90 degree 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | 19 | model://src_walkway_metal_straight 20 | 1.0 21 | 22 | 23 | 24 | 25 | A 90 degree turn metal walkway section. This model can be stitched together to create longer paths. 26 | SRC version 27 | 28 | 29 | -------------------------------------------------------------------------------- /models/src_walkway_metal_45/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | SRC Walkway Metal 45 degree 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Nate Koenig 10 | nate@osrfoundation.org 11 | 12 | 13 | 14 | Cole Biesemeyer 15 | 16 | 17 | 18 | 19 | model://walkway_metal_straight 20 | 1.0 21 | 22 | 23 | 24 | 25 | A 45 degree turn metal walkway section. This model can be stitched together to create longer paths. 26 | SRC version which uses polylines as collisions 27 | 28 | 29 | -------------------------------------------------------------------------------- /config/val_grasping.yaml: -------------------------------------------------------------------------------- 1 | # Publish all joint states ----------------------------------- 2 | hands_joint_state_controller: 3 | type: joint_state_controller/JointStateController 4 | publish_rate: 50 5 | 6 | # Position Controllers --------------------------------------- 7 | left_hand_position_controller: 8 | type: position_controllers/JointGroupPositionController 9 | joints: 10 | - leftThumbRoll 11 | - leftThumbPitch1 12 | - leftIndexFingerPitch1 13 | - leftMiddleFingerPitch1 14 | - leftPinkyPitch1 15 | right_hand_position_controller: 16 | type: position_controllers/JointGroupPositionController 17 | joints: 18 | - rightThumbRoll 19 | - rightThumbPitch1 20 | - rightIndexFingerPitch1 21 | - rightMiddleFingerPitch1 22 | - rightPinkyPitch1 23 | 24 | -------------------------------------------------------------------------------- /models/mars_explorer/meshes/mars_explorer.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 08.02.2017 20:14:35 3 | 4 | newmtl mars_explorer 5 | Ns 10.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.5882 0.5882 0.5882 12 | Kd 0.5882 0.5882 0.5882 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://mars_explorer/materials/textures/mars_explorer.png 16 | map_Kd model://mars_explorer/materials/textures/mars_explorer.png 17 | 18 | newmtl mars_explorer_trailer 19 | Ns 10.0000 20 | Ni 1.5000 21 | d 1.0000 22 | Tr 0.0000 23 | Tf 1.0000 1.0000 1.0000 24 | illum 2 25 | Ka 0.5882 0.5882 0.5882 26 | Kd 0.5882 0.5882 0.5882 27 | Ks 0.0000 0.0000 0.0000 28 | Ke 0.0000 0.0000 0.0000 29 | map_Ka model://mars_explorer/materials/textures/mars_explorer_trailer.png 30 | map_Kd model://mars_explorer/materials/textures/mars_explorer_trailer.png 31 | -------------------------------------------------------------------------------- /msg/Task.msg: -------------------------------------------------------------------------------- 1 | # Task id 2 | uint32 task 3 | 4 | # Id of current checkpoint 5 | uint32 current_checkpoint 6 | 7 | # Array with the total time taken to complete each checkpoint. The time is zero 8 | # for skipped checkpoints or checkpoints which were never started due to task 9 | # timeout. This doesn't include penalties. 10 | duration[] checkpoint_durations 11 | 12 | # Array with the total penalty time for each checkpoint. 13 | duration[] checkpoint_penalties 14 | 15 | # Absolute sim time when this task started (ROS time) 16 | time start_time 17 | 18 | # Time elapsed since task started (ROS time). This is the current sim time 19 | # minus start time, plus penalties for all checkpoints on this task. 20 | # When the elapsed time reaches the task timeout, `timed_out` is set to true. 21 | duration elapsed_time 22 | 23 | # True if task timed out. This happens when the elapsed time plus the penalty 24 | # time are larger than the timeout. 25 | bool timed_out 26 | 27 | # True if task has finished 28 | bool finished 29 | -------------------------------------------------------------------------------- /models/src_walkway_metal_90/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0.762 0.076075 0 0 0 8 | 9 | 10 | 1.524 1.524 0.15215 11 | 12 | 13 | 14 | 15 | 16 | 1e+06 17 | 100 18 | 1 19 | 0 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.001 0.001 0.001 29 | model://src_walkway_metal_90/meshes/mesh.obj 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /models/src_walkway_metal_straight/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0.762 0.076075 0 0 0 8 | 9 | 10 | 1.524 1.524 0.15215 11 | 12 | 13 | 14 | 15 | 16 | 1e+06 17 | 100 18 | 1 19 | 0 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.001 0.001 0.001 29 | model://src_walkway_metal_straight/meshes/mesh.obj 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /models/walkway_finish_adjacent/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 0.076075 0 0 0 8 | 9 | 10 | 3.048 3.048 0.15215 11 | 12 | 13 | 14 | 15 | 16 | 1e+06 17 | 100 18 | 1 19 | 0 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.001 0.001 0.001 29 | model://walkway_finish_adjacent/meshes/walkway_finish_adjacent.obj 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /models/walkway_finish_opposite/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 0.076075 0 0 0 8 | 9 | 10 | 3.048 3.048 0.15215 11 | 12 | 13 | 14 | 15 | 16 | 1e+06 17 | 100 18 | 1 19 | 0 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 0.001 0.001 0.001 29 | model://walkway_finish_opposite/meshes/walkway_finish_opposite.obj 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /scripts/init_robot.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" 4 | 5 | echo -e "\e[32mINIT:\e[0m Wait 15s (ROS time) for system to load" 6 | python $DIR/rossleep.py 15 7 | 8 | echo -e "\e[32mINIT:\e[0m Lower harness" 9 | rostopic pub -1 /valkyrie/harness/velocity std_msgs/Float32 '{data: -0.05}' 10 | 11 | echo -e "\e[32mINIT:\e[0m Wait 5s (ROS time) to be lowered" 12 | python $DIR/rossleep.py 5 13 | 14 | echo -e "\e[32mINIT:\e[0m Switch to high level control" 15 | rostopic pub -1 /ihmc_ros/valkyrie/control/low_level_control_mode ihmc_valkyrie_ros/ValkyrieLowLevelControlModeRosMessage '{requested_control_mode: 2, unique_id: -1}' 16 | 17 | echo -e "\e[32mINIT:\e[0m Wait 2s (ROS time) for control to be switched" 18 | python $DIR/rossleep.py 2 19 | 20 | echo -e "\e[32mINIT:\e[0m Detach from harness" 21 | rostopic pub -1 /valkyrie/harness/detach std_msgs/Bool true & 22 | 23 | if [ $1 = "true" ]; then 24 | echo -e "\e[32mINIT:\e[0m Start walking" 25 | rosrun srcsim walk_test.py 26 | fi 27 | 28 | echo -e "\e[32mINIT:\e[0m Done" 29 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | srcsim 7 | 0.0.1 8 | Space Robotics Challenge 9 | Nate Koenig 10 | Apache License 2.0 11 | 12 | catkin 13 | 14 | gazebo_plugins 15 | gazebo_ros 16 | roscpp 17 | 18 | message_generation 19 | controller_manager 20 | ihmc_valkyrie_ros 21 | joint_state_controller 22 | joint_state_publisher 23 | message_runtime 24 | position_controllers 25 | robot_state_publisher 26 | stereo_image_proc 27 | topic_tools 28 | val_deploy 29 | val_gazebo 30 | 31 | 32 | -------------------------------------------------------------------------------- /models/src_walkway_metal_45/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | 0.15215 10 | 11 | -0.762 0 12 | -0.762 1.07763 13 | 0 1.8396 14 | 1.07763 0.763 15 | 0.762 0.45 16 | 0.762 0 17 | 18 | 19 | 20 | 21 | 22 | 23 | 1e+06 24 | 100 25 | 1 26 | 0 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 0.001 0.001 0.001 36 | model://src_walkway_metal_45/meshes/mesh.obj 37 | 38 | 39 | 40 | 41 | 42 | 43 | -------------------------------------------------------------------------------- /models/src_habitat/meshes/hdu.mtl: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 06.04.2017 18:40:21 3 | 4 | newmtl lambert3SG 5 | Ns 30.0000 6 | Ni 1.5000 7 | d 1.0000 8 | Tr 0.0000 9 | Tf 1.0000 1.0000 1.0000 10 | illum 2 11 | Ka 0.0000 0.0000 0.0000 12 | Kd 1.0000 1.0000 1.0000 13 | Ks 0.0000 0.0000 0.0000 14 | Ke 0.0000 0.0000 0.0000 15 | map_Ka model://src_habitat/materials/textures/hdu_02.png 16 | map_Kd model://src_habitat/materials/textures/hdu_02.png 17 | 18 | newmtl lambert2SG 19 | Ns 30.0000 20 | Ni 1.5000 21 | d 1.0000 22 | Tr 0.0000 23 | Tf 1.0000 1.0000 1.0000 24 | illum 2 25 | Ka 0.0000 0.0000 0.0000 26 | Kd 1.0000 1.0000 1.0000 27 | Ks 0.0000 0.0000 0.0000 28 | Ke 0.0000 0.0000 0.0000 29 | map_Ka model://src_habitat/materials/textures/hdu_01.png 30 | map_Kd model://src_habitat/materials/textures/hdu_01.png 31 | 32 | newmtl Walkway_Finish 33 | Ns 10.0000 34 | Ni 1.5000 35 | d 1.0000 36 | Tr 0.0000 37 | Tf 1.0000 1.0000 1.0000 38 | illum 2 39 | Ka 0.5882 0.5882 0.5882 40 | Kd 0.5882 0.5882 0.5882 41 | Ks 0.0000 0.0000 0.0000 42 | Ke 0.0000 0.0000 0.0000 43 | map_Ka model://walkway_finish_opposite/materials/textures/walkway_finish.png 44 | map_Kd model://walkway_finish_opposite/materials/textures/walkway_finish.png 45 | -------------------------------------------------------------------------------- /models/solar_array/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | true 7 | 8 | 9 | 10 | 11 | 12 | 13 | model://solar_array/meshes/solar_array.obj 14 | 0.0254 0.0254 0.0254 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | model://solar_array/meshes/solar_array.obj 23 | 0.0252 0.0252 0.0252 24 | 25 | 26 | 27 | 28 | 29 | 0 0 0.25 0 0 0 30 | 31 | 32 | 1.498 0.99 0.897 33 | 34 | 35 | 36 | 37 | 38 | 1e6 39 | 1 40 | 0.1 41 | 0.0015 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | -------------------------------------------------------------------------------- /worlds/leak.erb: -------------------------------------------------------------------------------- 1 | <% 2 | # Template for leak model for SRC finals world 3 | 4 | # Requires the following global variables: 5 | # $habitat_frame, $table_pos_z, $leak_model_name 6 | 7 | # start and end points of the line 8 | pt_0 = $habitat_frame * TMatrix(-4.781079, 3.018834, 0) 9 | pt_f = $habitat_frame * TMatrix(-0.136121, 5.695208, 0) 10 | 11 | # where in the line 12 | r = rand(0.0...1.0) 13 | 14 | x = pt_0[0, 2] + r * (pt_f[0, 2] - pt_0[0, 2]) 15 | y = pt_0[1, 2] + r * (pt_f[1, 2] - pt_0[1, 2]) 16 | 17 | # height 18 | min_height = $table_pos_z + 0.635 19 | max_height = min_height + 0.878 20 | 21 | z = rand(min_height...max_height) 22 | 23 | %> 24 | 25 | 26 | true 27 | 28 | <%= x %> <%= y %> <%= z %> 0 0 0 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 0.02 37 | 38 | 39 | 40 | 41 | 42 | 43 | 0x10000 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | collision 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /launch/multisense.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 12 | 14 | 15 | 16 | 17 | 20 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /models/air_leak_detector/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 0 11 | -0.0008 12 | 0.03399 13 | 0 14 | 0 15 | 0 16 | 17 | 18 | 0.005172208333333333 19 | 0.0009736016666666666 20 | 0.005377408333333333 21 | 22 | 1.0 23 | 24 | 25 | 26 | 27 | 28 | 0.0254 0.0254 0.0254 29 | model://air_leak_detector/meshes/air_leak_detector.obj 30 | 31 | 32 | 33 | 34 | 35 | 1e5 36 | 1 37 | 0.1 38 | 0.0015 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 0.0254 0.0254 0.0254 48 | model://air_leak_detector/meshes/air_leak_detector.obj 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /models/src_walkway_metal_straight/meshes/mesh.obj: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 30.11.2016 17:16:45 3 | 4 | mtllib mesh.mtl 5 | 6 | # 7 | # object Walkway_Straight 8 | # 9 | 10 | v -762.0000 0.0000 0.0000 11 | v -762.0000 1524.0000 0.0000 12 | v 762.0000 1524.0000 0.0000 13 | v 762.0000 0.0000 0.0000 14 | v -762.0000 0.0000 152.1460 15 | v 762.0000 0.0000 152.1460 16 | v 762.0000 1524.0000 152.1460 17 | v -762.0000 1524.0000 152.1460 18 | # 8 vertices 19 | 20 | vn 0.0000 0.0000 -1.0000 21 | vn 0.0000 0.0000 1.0000 22 | vn 1.0000 0.0000 0.0000 23 | vn -1.0000 0.0000 0.0000 24 | vn 0.0000 -1.0000 0.0000 25 | vn 0.0000 1.0000 0.0000 26 | # 6 vertex normals 27 | 28 | vt 1.0000 0.0000 0.0000 29 | vt 0.0000 0.0000 0.0000 30 | vt 1.0000 1.0000 0.0000 31 | vt 0.0000 1.0000 0.0000 32 | vt 0.0000 0.0000 0.0000 33 | vt 1.0000 0.0000 0.0000 34 | vt 0.0000 1.0000 0.0000 35 | vt 1.0000 1.0000 0.0000 36 | vt 0.9056 0.9999 0.0000 37 | vt 0.9056 -0.0001 0.0000 38 | vt 0.9951 0.9999 0.0000 39 | vt 0.9951 -0.0001 0.0000 40 | vt 0.9056 0.9999 0.0000 41 | vt 0.9056 -0.0001 0.0000 42 | vt 0.9951 0.9999 0.0000 43 | vt 0.9951 -0.0001 0.0000 44 | vt 0.0000 -0.0895 0.0000 45 | vt 1.0000 -0.0895 0.0000 46 | vt 1.0000 1.0895 0.0000 47 | vt 0.0000 1.0895 0.0000 48 | # 20 texture coords 49 | 50 | g Walkway_Straight 51 | usemtl Walkway 52 | s off 53 | f 1/1/1 2/3/1 3/4/1 54 | f 3/4/1 4/2/1 1/1/1 55 | f 5/5/2 6/6/2 7/8/2 56 | f 7/8/2 8/7/2 5/5/2 57 | f 4/9/3 3/10/3 7/12/3 58 | f 7/12/3 6/11/3 4/9/3 59 | f 2/13/4 1/14/4 5/16/4 60 | f 5/16/4 8/15/4 2/13/4 61 | f 1/17/5 4/18/5 6/6/5 62 | f 6/6/5 5/5/5 1/17/5 63 | f 3/19/6 2/20/6 8/7/6 64 | f 8/7/6 7/8/6 3/19/6 65 | # 12 faces 66 | 67 | -------------------------------------------------------------------------------- /models/src_walkway_metal_90/meshes/mesh.obj: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 30.11.2016 17:17:52 3 | 4 | mtllib mesh.mtl 5 | 6 | # 7 | # object Walkway_90_Right 8 | # 9 | 10 | v -762.0000 1524.0000 0.0000 11 | v 762.0000 1524.0000 0.0000 12 | v 762.0000 0.0000 0.0000 13 | v 762.0000 0.0000 152.1460 14 | v 762.0000 1524.0000 152.1460 15 | v -762.0000 1524.0000 152.1460 16 | v -762.0000 0.0000 0.0000 17 | v -762.0000 0.0000 152.1460 18 | # 8 vertices 19 | 20 | vn 0.0000 0.0000 -1.0000 21 | vn 0.0000 0.0000 1.0000 22 | vn -1.0000 0.0000 0.0000 23 | vn 0.0000 1.0000 0.0000 24 | vn 0.0000 -1.0000 0.0000 25 | vn 1.0000 0.0000 0.0000 26 | # 6 vertex normals 27 | 28 | vt 1.0000 0.0000 0.0000 29 | vt 0.0000 0.0000 0.0000 30 | vt 1.0000 1.0000 0.0000 31 | vt 0.0000 0.0000 0.0000 32 | vt 0.0000 0.0000 0.0000 33 | vt 1.0000 0.0000 0.0000 34 | vt 0.0000 1.0000 0.0000 35 | vt 0.0000 1.0000 0.0000 36 | vt 0.9056 0.9999 0.0000 37 | vt 0.9056 -0.0001 0.0000 38 | vt 0.9951 0.9999 0.0000 39 | vt 0.9951 -0.0001 0.0000 40 | vt 0.9056 0.9999 0.0000 41 | vt 0.9056 -0.0001 0.0000 42 | vt 0.9951 -0.0001 0.0000 43 | vt 0.9951 0.9999 0.0000 44 | vt 1.0000 -0.0000 0.0000 45 | vt 0.0000 1.0000 0.0000 46 | vt 1.0000 1.0000 0.0000 47 | vt 0.0000 0.0000 0.0000 48 | vt 0.0000 -0.0895 0.0000 49 | vt 1.0000 -0.0895 0.0000 50 | vt 1.0000 1.0895 0.0000 51 | vt 0.0000 1.0895 0.0000 52 | # 24 texture coords 53 | 54 | g Walkway_90_Right 55 | usemtl Walkway 56 | s off 57 | f 1/18/1 2/4/1 3/17/1 58 | f 4/19/2 5/8/2 6/20/2 59 | f 1/9/3 7/10/3 8/12/3 60 | f 8/12/3 6/11/3 1/9/3 61 | f 6/7/2 8/5/2 4/6/2 62 | f 3/2/1 7/1/1 1/3/1 63 | f 2/13/4 1/14/4 6/15/4 64 | f 6/15/4 5/16/4 2/13/4 65 | f 7/21/5 3/22/5 4/6/5 66 | f 4/6/5 8/5/5 7/21/5 67 | f 3/23/6 2/24/6 5/8/6 68 | f 5/8/6 4/19/6 3/23/6 69 | # 12 faces 70 | 71 | -------------------------------------------------------------------------------- /models/heightmap_mars/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 8 | 9 | model://heightmap_mars/materials/textures/marssmall.jpg 10 | 100 100 15 11 | 0 -4 0 12 | 13 | 14 | 15 | 16 | 17 | 18 | false 19 | 20 | 21 | model://heightmap_mars/materials/textures/mars_tex1.jpg 22 | file://media/materials/textures/flat_normal.png 23 | 10 24 | 25 | 26 | 27 | 0 28 | 0.5 29 | 30 | 31 | 32 | model://heightmap_mars/materials/textures/mars_tex2.jpg 33 | file://media/materials/textures/flat_normal.png 34 | 10 35 | 36 | 37 | 38 | 1 39 | 5 40 | 41 | 42 | 43 | model://heightmap_mars/materials/textures/mars_tex3.jpg 44 | file://media/materials/textures/flat_normal.png 45 | 10 46 | 47 | 48 | model://heightmap_mars/materials/textures/marssmall.jpg 49 | 100 100 15 50 | 0 -4 0 51 | 52 | 53 | 54 | 55 | 56 | 57 | -------------------------------------------------------------------------------- /launch/qual1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 41 | 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /worlds/qual2.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 1000.0 7 | 0.001 8 | 1 9 | 10 | 11 | quick 12 | 150 13 | 0 14 | 1.400000 15 | 1 16 | 17 | 18 | 0.000000 19 | 0.800000 20 | 2000.000000 21 | 0.01000 22 | 23 | 24 | 25 | 26 | 27 | 28 | -3.5 -3.5 3 0 0.34 0.588 29 | 30 | 31 | 32 | 33 | 34 | true 35 | 0 0 10 0 0 0 36 | 0.9 0.9 0.9 1 37 | 0.2 0.2 0.2 1 38 | 39 | 1000 40 | 0.9 41 | 0.01 42 | 0.001 43 | 44 | 0.5 0.1 -0.7 45 | 46 | 47 | 48 | 49 | model://ground_plane 50 | 51 | 52 | 53 | 54 | 3.5 0 0 0 0 1.5708 55 | 56 | model://src_doorway_q2 57 | 58 | 59 | 60 | src_doorway_q2::hinge 61 | src_doorway_q2::button_mechanism 62 | 63 | 64 | 65 | 66 | -------------------------------------------------------------------------------- /launch/unique.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /launch/qual2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /launch/unique_task1.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /launch/unique_task2.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /launch/unique_task3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 44 | 45 | 46 | 47 | 48 | 49 | -------------------------------------------------------------------------------- /include/srcsim/Qual2Plugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #ifndef GAZEBO_PLUGINS_QUAL2PLUGIN_HH_ 18 | #define GAZEBO_PLUGINS_QUAL2PLUGIN_HH_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace gazebo 27 | { 28 | class GAZEBO_VISIBLE Qual2Plugin : public ModelPlugin 29 | { 30 | /// \brief Constructor 31 | public: Qual2Plugin(); 32 | 33 | // Documentation inherited 34 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 35 | 36 | /// \brief Whether the button is pressed. 37 | public: bool Pressed() const; 38 | 39 | /// \brief Open the door. 40 | public: void OpenTheDoor(); 41 | 42 | /// \brief Close the door. 43 | public: void CloseTheDoor(); 44 | 45 | /// \brief Update plugin's function. 46 | private: void OnUpdate(); 47 | 48 | /// \brief Pointer to the joint that actuates the button. 49 | private: physics::JointPtr buttonJoint; 50 | 51 | /// \brief Pointer to the joint that opens/closes the door (the hinge). 52 | private: physics::JointPtr hingeJoint; 53 | 54 | /// \brief Whether the button is pressed. 55 | private: bool pressed = false; 56 | 57 | /// \brief Lower limit of the button's joint. 58 | private: double lowerLimit; 59 | 60 | /// \brief Upper limit of the button's joint. 61 | private: double upperLimit; 62 | 63 | /// \brief The range of motion of the button's joint. 64 | private: double range; 65 | 66 | /// \brief The door is opening. 67 | private: bool opening = false; 68 | 69 | /// \brief Between 0% of the range and this value the button is considered 70 | /// not pressed. From this value to 100% the button is considered pressed. 71 | private: static const int kPercentageButtonPressed = 75; 72 | 73 | /// \brief Connection to World Update events. 74 | private: event::ConnectionPtr updateConnection; 75 | }; 76 | } 77 | 78 | #endif 79 | -------------------------------------------------------------------------------- /scripts/neck_traj_yaml.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import copy 3 | import time 4 | import rospy 5 | import sys 6 | import yaml 7 | 8 | from math import ceil 9 | from numpy import append, array, linspace, zeros 10 | 11 | from ihmc_msgs.msg import NeckTrajectoryRosMessage 12 | from ihmc_msgs.msg import OneDoFJointTrajectoryRosMessage 13 | from ihmc_msgs.msg import TrajectoryPoint1DRosMessage 14 | 15 | def sendTrajectory(joint_waypoints): 16 | msg = NeckTrajectoryRosMessage() 17 | msg.unique_id = -1 18 | # for each set of joint states 19 | for y in joint_waypoints: 20 | # first value is time duration 21 | time = float(y[0]) 22 | # subsequent values are desired joint positions 23 | commandPosition = array([ float(x) for x in y[1].split() ]) 24 | msg = appendTrajectoryPoint(msg, time, commandPosition) 25 | rospy.loginfo('publishing neck trajectory') 26 | neckTrajectoryPublisher.publish(msg) 27 | 28 | def appendTrajectoryPoint(neck_trajectory, time, positions): 29 | if not neck_trajectory.joint_trajectory_messages: 30 | neck_trajectory.joint_trajectory_messages = [copy.deepcopy(OneDoFJointTrajectoryRosMessage()) for i in range(len(positions))] 31 | for i, pos in enumerate(positions): 32 | point = TrajectoryPoint1DRosMessage() 33 | point.time = time 34 | point.position = pos 35 | point.velocity = 0 36 | neck_trajectory.joint_trajectory_messages[i].trajectory_points.append(point) 37 | return neck_trajectory 38 | 39 | if __name__ == '__main__': 40 | # first make sure the input arguments are correct 41 | if len(sys.argv) != 3: 42 | print "usage: neck_traj_yaml.py YAML_FILE TRAJECTORY_NAME" 43 | print " where TRAJECTORY is a dictionary defined in YAML_FILE" 44 | sys.exit(1) 45 | traj_yaml = yaml.load(file(sys.argv[1], 'r')) 46 | traj_name = sys.argv[2] 47 | if not traj_name in traj_yaml: 48 | print "unable to find trajectory %s in %s" % (traj_name, sys.argv[1]) 49 | sys.exit(1) 50 | traj = traj_yaml[traj_name] 51 | 52 | try: 53 | rospy.init_node('ihmc_neck_control_yaml') 54 | 55 | ROBOT_NAME = rospy.get_param('/ihmc_ros/robot_name') 56 | 57 | neckTrajectoryPublisher = rospy.Publisher("/ihmc_ros/{0}/control/neck_trajectory".format(ROBOT_NAME), NeckTrajectoryRosMessage, queue_size=1) 58 | 59 | rate = rospy.Rate(10) # 10hz 60 | 61 | # make sure the simulation is running otherwise wait 62 | if neckTrajectoryPublisher.get_num_connections() == 0: 63 | rospy.loginfo('waiting for subsciber...') 64 | while neckTrajectoryPublisher.get_num_connections() == 0: 65 | rate.sleep() 66 | 67 | if not rospy.is_shutdown(): 68 | sendTrajectory(traj) 69 | 70 | except rospy.ROSInterruptException: 71 | pass 72 | -------------------------------------------------------------------------------- /models/src_walkway_metal_45/meshes/mesh.obj: -------------------------------------------------------------------------------- 1 | # 3ds Max Wavefront OBJ Exporter v0.97b - (c)2007 guruware 2 | # File Created: 30.11.2016 17:17:21 3 | 4 | mtllib mesh.mtl 5 | 6 | # 7 | # object Walkway_45_Right 8 | # 9 | 10 | v -762.0001 1077.6306 -0.1269 11 | v -762.0001 1077.6306 152.0191 12 | v 0.1976 1839.5492 152.1460 13 | v 0.1976 1839.5492 0.0000 14 | v 1077.8284 761.9188 0.0000 15 | v 762.0001 446.3694 -0.1271 16 | v 762.0001 446.3694 152.0189 17 | v 1077.8284 761.9188 152.1460 18 | v -762.0000 -0.0001 151.8921 19 | v 762.0001 0.0000 151.8919 20 | v -762.0000 -0.0001 -0.2539 21 | v 762.0001 0.0000 -0.2541 22 | # 12 vertices 23 | 24 | vn -0.7070 0.7072 -0.0000 25 | vn -0.7070 0.7072 0.0000 26 | vn 0.0002 0.0002 -1.0000 27 | vn 0.0000 0.0001 -1.0000 28 | vn -0.0002 -0.0002 1.0000 29 | vn -0.0000 -0.0001 1.0000 30 | vn 0.7068 -0.7074 0.0000 31 | vn 0.0000 -0.0003 1.0000 32 | vn -0.0000 0.0003 -1.0000 33 | vn -1.0000 -0.0000 0.0000 34 | vn 1.0000 0.0000 0.0000 35 | vn 0.0000 -1.0000 0.0000 36 | vn 0.7071 0.7071 0.0000 37 | # 13 vertex normals 38 | 39 | vt 1.0000 1.0000 0.0000 40 | vt 0.0000 1.0000 0.0000 41 | vt 0.0000 1.0000 0.0000 42 | vt 1.0000 1.0000 0.0000 43 | vt 0.9056 -0.0001 0.0000 44 | vt 0.9951 -0.0001 0.0000 45 | vt 0.9056 0.9999 0.0000 46 | vt 0.9951 0.9999 0.0000 47 | vt 1.0000 0.2929 0.0000 48 | vt 0.9056 0.2928 0.0000 49 | vt 0.0000 0.7071 0.0000 50 | vt 0.9056 0.2928 0.0000 51 | vt 1.0000 0.7071 0.0000 52 | vt 0.9951 0.2928 0.0000 53 | vt 0.0000 0.2929 0.0000 54 | vt 0.9951 0.2928 0.0000 55 | vt 1.0000 0.0000 0.0000 56 | vt 0.0000 0.0000 0.0000 57 | vt 0.0000 0.0000 0.0000 58 | vt 1.0000 0.0000 0.0000 59 | vt 0.9951 0.9999 0.0000 60 | vt 0.9056 0.9999 0.0000 61 | vt 0.9951 -0.0001 0.0000 62 | vt 0.9056 -0.0001 0.0000 63 | vt 1.0000 0.2929 0.0000 64 | vt 0.9951 0.2928 0.0000 65 | vt 0.0000 0.7071 0.0000 66 | vt 0.9951 0.2928 0.0000 67 | vt 1.0000 0.7071 0.0000 68 | vt 0.9056 0.2928 0.0000 69 | vt 0.0000 0.2929 0.0000 70 | vt 0.9056 0.2928 0.0000 71 | vt 0.0000 0.1266 0.0000 72 | vt 1.0000 0.1266 0.0000 73 | vt 1.0000 0.8734 0.0000 74 | vt 0.0000 0.8734 0.0000 75 | # 36 texture coords 76 | 77 | g Walkway_45_Right 78 | usemtl Walkway 79 | s off 80 | f 1/10/1 2/16/1 3/8/1 81 | f 3/8/2 4/7/2 1/10/2 82 | f 4/1/3 5/2/3 6/11/3 83 | f 6/11/4 1/9/4 4/1/4 84 | f 7/13/5 8/4/5 3/3/5 85 | f 3/3/6 2/15/6 7/13/6 86 | f 5/5/7 8/6/7 7/14/7 87 | f 7/14/7 6/12/7 5/5/7 88 | f 7/25/6 2/27/6 9/18/6 89 | f 9/18/8 10/17/8 7/25/8 90 | f 11/20/4 1/29/4 6/31/4 91 | f 6/31/9 12/19/9 11/20/9 92 | f 2/28/10 1/30/10 11/22/10 93 | f 11/22/10 9/21/10 2/28/10 94 | f 10/23/11 12/24/11 6/32/11 95 | f 6/32/11 7/26/11 10/23/11 96 | f 9/18/12 11/33/12 12/34/12 97 | f 12/34/12 10/17/12 9/18/12 98 | f 8/4/13 5/35/13 4/36/13 99 | f 4/36/13 3/3/13 8/4/13 100 | # 20 faces 101 | 102 | -------------------------------------------------------------------------------- /scripts/cheat_detect.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | from gazebo_msgs.msg import LinkStates 6 | from gazebo_msgs.msg import LinkState 7 | from gazebo_msgs.msg import ModelStates 8 | from gazebo_msgs.msg import ModelState 9 | from dynamic_reconfigure.msg import Config 10 | from dynamic_reconfigure.msg import ConfigDescription 11 | from std_msgs.msg import Float32 12 | from std_msgs.msg import Bool 13 | from std_msgs.msg import Empty 14 | from geometry_msgs.msg import Pose 15 | 16 | 17 | class CheatDetectNode(object): 18 | def __init__(self): 19 | rospy.init_node("GazeboHelper") 20 | cheat_pubs = { 21 | "/gazebo/link_states": LinkStates, 22 | "/gazebo/model_states": ModelStates, 23 | "/gazebo/parameter_descriptions": ConfigDescription, 24 | "/gazebo/parameter_updates": Config, 25 | } 26 | 27 | cheat_subs = { 28 | "/gazebo/set_link_state": LinkState, 29 | "/valkyrie/harness/velocity": Float32, 30 | "/gazebo/set_model_state": ModelState, 31 | "/valkyrie/harness/attach": Pose, 32 | "/srcsim/finals/force_checkpoint_completion": Empty, 33 | } 34 | 35 | self.pubs = list() 36 | self.subs = list() 37 | self.pub_counts = [] 38 | self.sub_counts = [] 39 | for name, message_type in cheat_pubs.items(): 40 | pub_info = {"name": name, "pub": None, } 41 | pub_info["pub"] = rospy.Publisher(name, message_type, queue_size=1) 42 | self.pubs.append(pub_info) 43 | self.pub_counts.append(0) 44 | for name, message_type in cheat_subs.items(): 45 | sub_info = {"name": name, "sub": None} 46 | sub_info["sub"] = rospy.Subscriber(name, message_type, self._callback) 47 | self.subs.append(sub_info) 48 | self.sub_counts.append(0) 49 | 50 | def _callback(self, msg): 51 | rospy.logerr("Cheat detected (received message): %s", msg) 52 | 53 | def wait_for_cheaters(self): 54 | while not rospy.is_shutdown(): 55 | for idx, pub in enumerate(self.pubs): 56 | if pub["pub"].get_num_connections() != self.pub_counts[idx]: 57 | rospy.logerr("Cheat detected (something subscribed) %r", pub["pub"].impl.get_stats()) 58 | self.pub_counts[idx] = pub["pub"].get_num_connections() 59 | for idx, sub in enumerate(self.subs): 60 | if sub["sub"].get_num_connections() != self.sub_counts[idx]: 61 | rospy.logerr("Cheat detected (something published) %r", sub["sub"].impl.get_stats()) 62 | self.sub_counts[idx] = sub["sub"].get_num_connections() 63 | rospy.sleep(0.1) 64 | 65 | 66 | if __name__ == '__main__': 67 | try: 68 | node = CheatDetectNode() 69 | node.wait_for_cheaters() 70 | except rospy.ROSInterruptException: 71 | pass 72 | -------------------------------------------------------------------------------- /scripts/head_traj_yaml.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import copy 3 | import time 4 | import rospy 5 | import sys 6 | import tf 7 | import yaml 8 | 9 | from math import ceil 10 | from numpy import append, array, linspace, zeros 11 | 12 | from geometry_msgs.msg import Quaternion 13 | from geometry_msgs.msg import Vector3 14 | from ihmc_msgs.msg import HeadTrajectoryRosMessage 15 | from ihmc_msgs.msg import SO3TrajectoryPointRosMessage 16 | 17 | def sendTrajectory(head_waypoints): 18 | msg = HeadTrajectoryRosMessage() 19 | msg.unique_id = -1 20 | # for each set of joint states 21 | for y in head_waypoints: 22 | # first value is time duration 23 | time = float(y[0]) 24 | # subsequent values are desired joint commands 25 | commands = array([ float(x) for x in y[1].split() ]) 26 | msg = appendTrajectoryPoint(msg, time, commands) 27 | rospy.loginfo('publishing %s trajectory' % "head") 28 | headTrajectoryPublisher.publish(msg) 29 | 30 | def appendTrajectoryPoint(head_trajectory, time, rollPitchYaw): 31 | roll = rollPitchYaw[0] 32 | pitch = rollPitchYaw[1] 33 | yaw = rollPitchYaw[2] 34 | quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw) 35 | point = copy.deepcopy(SO3TrajectoryPointRosMessage()) 36 | point.time = time 37 | point.orientation = copy.deepcopy(Quaternion()) 38 | point.orientation.x = quat[0] 39 | point.orientation.y = quat[1] 40 | point.orientation.z = quat[2] 41 | point.orientation.w = quat[3] 42 | point.angular_velocity = copy.deepcopy(Vector3()) 43 | point.angular_velocity.x = 0 44 | point.angular_velocity.y = 0 45 | point.angular_velocity.z = 0 46 | head_trajectory.taskspace_trajectory_points.append(point) 47 | return head_trajectory 48 | 49 | if __name__ == '__main__': 50 | # first make sure the input arguments are correct 51 | if len(sys.argv) != 3: 52 | print "usage: head_traj_yaml.py YAML_FILE TRAJECTORY_NAME" 53 | print " where TRAJECTORY is a dictionary defined in YAML_FILE" 54 | sys.exit(1) 55 | traj_yaml = yaml.load(file(sys.argv[1], 'r')) 56 | traj_name = sys.argv[2] 57 | if not traj_name in traj_yaml: 58 | print "unable to find trajectory %s in %s" % (traj_name, sys.argv[1]) 59 | sys.exit(1) 60 | traj = traj_yaml[traj_name] 61 | 62 | try: 63 | rospy.init_node('ihmc_head_control') 64 | 65 | ROBOT_NAME = rospy.get_param('/ihmc_ros/robot_name') 66 | 67 | headTrajectoryPublisher = rospy.Publisher("/ihmc_ros/{0}/control/head_trajectory".format(ROBOT_NAME), HeadTrajectoryRosMessage, queue_size=1) 68 | 69 | rate = rospy.Rate(10) # 10hz 70 | 71 | # make sure the simulation is running otherwise wait 72 | if headTrajectoryPublisher.get_num_connections() == 0: 73 | rospy.loginfo('waiting for subsciber...') 74 | while headTrajectoryPublisher.get_num_connections() == 0: 75 | rate.sleep() 76 | 77 | if not rospy.is_shutdown(): 78 | sendTrajectory(traj) 79 | 80 | except rospy.ROSInterruptException: 81 | pass 82 | -------------------------------------------------------------------------------- /launch/finals.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | -------------------------------------------------------------------------------- /include/srcsim/FinalsPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include 19 | #include 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "Task.hh" 31 | 32 | namespace gazebo 33 | { 34 | class FinalsPlugin : public WorldPlugin 35 | { 36 | // Documentation inherited 37 | public: FinalsPlugin(); 38 | 39 | // Documentation inherited 40 | public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf); 41 | 42 | /// \brief Start a specific task and checkpoint. 43 | /// \param[in] _req Request 44 | /// \param[in] _res Response 45 | private: bool OnStartTaskRosRequest(srcsim::StartTask::Request &_req, 46 | srcsim::StartTask::Response &_res); 47 | 48 | /// \brief Update on world update begin 49 | /// \param[in] _info Update info 50 | private: void OnUpdate(const common::UpdateInfo &_info); 51 | 52 | /// \brief Callback when a task message is received. 53 | /// \param[in] _msg Task message 54 | private: void OnTaskRosMsg(const srcsim::Task::ConstPtr &_msg); 55 | 56 | /// \brief Connection to world update 57 | private: event::ConnectionPtr updateConnection; 58 | 59 | /// \brief Vector of tasks. 60 | /// tasks[0]: Task 1 61 | /// tasks[1]: Task 2 62 | /// tasks[2]: Task 3 63 | private: std::vector > tasks; 64 | 65 | /// \brief Current task number, starting from 1. Zero means no task. 66 | private: uint8_t current = 0; 67 | 68 | /// \brief Pointer to the world 69 | private: physics::WorldPtr world; 70 | 71 | /// \brief Ros node handle 72 | private: std::unique_ptr rosNode; 73 | 74 | /// \brief Ros task service server 75 | private: ros::ServiceServer startTaskRosService; 76 | 77 | /// \brief Ros publisher which publishes the score. 78 | private: ros::Publisher scoreRosPub; 79 | 80 | /// \brief Ros task subscriber 81 | private: ros::Subscriber taskRosSub; 82 | 83 | /// \brief Product used to know which tasks are in the world 84 | private: int product; 85 | 86 | /// \brief Time to wait before deharnessing for the first time (give 87 | /// controllers time to start). It is 15 seconds by default. 88 | private: gazebo::common::Time deharnessTime = 15.0; 89 | }; 90 | } 91 | -------------------------------------------------------------------------------- /scoring/scoring_q2.rb: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env ruby 2 | 3 | require 'nokogiri' 4 | require './common' 5 | 6 | # Parse an entire log containing states. 7 | class State 8 | def initialize(file) 9 | # Open and read the log file 10 | doc = Nokogiri::XML(File.open(file)) 11 | 12 | # Get the first chunk 13 | chunks = doc.xpath('//gazebo_log/chunk') 14 | if chunks.size < 2 15 | puts 'State log file has less than 2 entries.' 16 | return 17 | end 18 | 19 | start_line_crossed = false 20 | finish_line_crossed = false 21 | 22 | @events = {} 23 | 24 | # Create hash of the robot pose over time 25 | @poses = {} 26 | for i in 1..chunks.size - 1 27 | chunk = Nokogiri::XML(chunks[i].text) 28 | 29 | # Read the sim time 30 | parts = chunk.xpath('//sdf/state/sim_time').text.split 31 | time = Common::Time.new 32 | time.sec = parts[0].to_i 33 | time.nsec = parts[1].to_i 34 | 35 | # Read model pose 36 | model_pose = Common::Pose.new 37 | pose = chunk.xpath("//sdf/state/model[@name='valkyrie']/pose") 38 | next unless pose.size == 1 39 | 40 | parts = pose.text.split 41 | model_pose.set(parts[0].to_f, parts[1].to_f, parts[2].to_f, 42 | parts[3].to_f, parts[4].to_f, parts[5].to_f) 43 | 44 | if model_pose.p.x >= 0.5 && !start_line_crossed 45 | @events['start_line'] = time 46 | start_line_crossed = true 47 | elsif model_pose.p.x >= 4.5 && !finish_line_crossed 48 | @events['finish_line'] = time 49 | finish_line_crossed = true 50 | end 51 | end 52 | end 53 | 54 | def event(tag) 55 | time = Time.new 56 | found = false 57 | if @events.key?(tag) 58 | time = @events[tag] 59 | found = true 60 | end 61 | 62 | return found, time 63 | end 64 | end 65 | 66 | # Sanity check: Make sure that the Gazebo log is passed as an argument. 67 | if ARGV.size != 1 68 | puts 'Usage: scoring.rb ' 69 | exit(-1) 70 | end 71 | 72 | state_log = ARGV[0] 73 | 74 | # Sanity check: Make sure that the Gazebo log exists. 75 | unless File.file?(state_log) 76 | puts 'Invalid state log file' 77 | exit(-1) 78 | end 79 | 80 | # Read all the state information 81 | state = State.new(state_log) 82 | 83 | # Did the robot crossed the starting line? 84 | start_line_crossed, start_time = state.event('start_line') 85 | 86 | # Did the robot crossed the finish line? 87 | finish_line_crossed, finish_time = state.event('finish_line') 88 | 89 | if start_line_crossed 90 | printf("Start line crossed: [%d %09d]\n", start_time.sec, start_time.nsec) 91 | else 92 | printf('Start line crossed: --') 93 | end 94 | 95 | if finish_line_crossed 96 | printf("Finish line crossed: [%d %09d]\n", finish_time.sec, finish_time.nsec) 97 | else 98 | printf('Finish line crossed: --') 99 | end 100 | 101 | if start_line_crossed && finish_line_crossed 102 | elapsed = finish_time - start_time 103 | printf("Elapsed time: [%d %09d]\n", elapsed.sec, elapsed.nsec) 104 | else 105 | printf('Elapsed time: --') 106 | end 107 | 108 | exit(0) 109 | -------------------------------------------------------------------------------- /scripts/arm_traj_yaml.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import copy 3 | import time 4 | import rospy 5 | import sys 6 | import yaml 7 | 8 | from math import ceil 9 | from numpy import append, array, linspace, zeros 10 | 11 | from ihmc_msgs.msg import ArmTrajectoryRosMessage 12 | from ihmc_msgs.msg import OneDoFJointTrajectoryRosMessage 13 | from ihmc_msgs.msg import TrajectoryPoint1DRosMessage 14 | 15 | def sendTrajectory(joint_waypoints): 16 | msgs = {} 17 | msgs["left"] = copy.deepcopy(ArmTrajectoryRosMessage()) 18 | msgs["right"] = copy.deepcopy(ArmTrajectoryRosMessage()) 19 | msgs["left"].robot_side = ArmTrajectoryRosMessage.LEFT 20 | msgs["right"].robot_side = ArmTrajectoryRosMessage.RIGHT 21 | # for each set of joint states 22 | for y in joint_waypoints: 23 | # first value is time duration 24 | time = float(y[0]) 25 | side = str(y[1]).lower() 26 | # subsequent values are desired joint positions 27 | commandPosition = array([ float(x) for x in y[2].split() ]) 28 | if side in msgs.keys(): 29 | msgs[side] = appendTrajectoryPoint(msgs[side], time, commandPosition) 30 | msgs[side].unique_id = -1 31 | for side in msgs.keys(): 32 | if msgs[side].unique_id == -1: 33 | rospy.loginfo('publishing %s trajectory' % side) 34 | armTrajectoryPublisher.publish(msgs[side]) 35 | 36 | def appendTrajectoryPoint(arm_trajectory, time, positions): 37 | if not arm_trajectory.joint_trajectory_messages: 38 | arm_trajectory.joint_trajectory_messages = [copy.deepcopy(OneDoFJointTrajectoryRosMessage()) for i in range(len(positions))] 39 | for i, pos in enumerate(positions): 40 | point = TrajectoryPoint1DRosMessage() 41 | point.time = time 42 | point.position = pos 43 | point.velocity = 0 44 | arm_trajectory.joint_trajectory_messages[i].trajectory_points.append(point) 45 | return arm_trajectory 46 | 47 | if __name__ == '__main__': 48 | # first make sure the input arguments are correct 49 | if len(sys.argv) != 3: 50 | print "usage: arm_traj_yaml.py YAML_FILE TRAJECTORY_NAME" 51 | print " where TRAJECTORY is a dictionary defined in YAML_FILE" 52 | sys.exit(1) 53 | traj_yaml = yaml.load(file(sys.argv[1], 'r')) 54 | traj_name = sys.argv[2] 55 | if not traj_name in traj_yaml: 56 | print "unable to find trajectory %s in %s" % (traj_name, sys.argv[1]) 57 | sys.exit(1) 58 | traj = traj_yaml[traj_name] 59 | 60 | try: 61 | rospy.init_node('ihmc_arm_control_yaml') 62 | 63 | ROBOT_NAME = rospy.get_param('/ihmc_ros/robot_name') 64 | 65 | armTrajectoryPublisher = rospy.Publisher("/ihmc_ros/{0}/control/arm_trajectory".format(ROBOT_NAME), ArmTrajectoryRosMessage, queue_size=2) 66 | 67 | rate = rospy.Rate(10) # 10hz 68 | 69 | # make sure the simulation is running otherwise wait 70 | if armTrajectoryPublisher.get_num_connections() == 0: 71 | rospy.loginfo('waiting for subscriber...') 72 | while armTrajectoryPublisher.get_num_connections() == 0: 73 | rate.sleep() 74 | 75 | if not rospy.is_shutdown(): 76 | sendTrajectory(traj) 77 | 78 | except rospy.ROSInterruptException: 79 | pass 80 | -------------------------------------------------------------------------------- /include/srcsim/srcsim_ros_harness.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #ifndef SRCSIM_ROS_HARNESS_H 18 | #define SRCSIM_ROS_HARNESS_H 19 | 20 | // Custom Callback Queue 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace gazebo 32 | { 33 | /// \brief See the Gazebo documentation about the HarnessPlugin. This ROS 34 | /// wrapper exposes three topics: 35 | /// 36 | /// 1. //harness/velocity 37 | /// - Message Type: std_msgs::Float32 38 | /// - Purpose: Set target winch velocity 39 | /// 40 | /// 2. //harness/detach 41 | /// - Message Type: std_msgs::Bool 42 | /// - Purpose: Detach the joint. 43 | /// 44 | /// 3. //harness/attach 45 | /// - Message Type: geometry_msgs::Pose 46 | /// - Purpose: Teleport the model to specified pose and re-attach harness. 47 | class SRCSimRosHarness : public SRCHarnessPlugin 48 | { 49 | /// \brief Constructor 50 | public: SRCSimRosHarness(); 51 | 52 | /// \brief Destructor 53 | public: virtual ~SRCSimRosHarness(); 54 | 55 | // Documentation inherited 56 | public: virtual void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); 57 | 58 | /// \brief Receive winch velocity control messages. 59 | /// \param[in] msg Float message that is the target winch velocity. 60 | private: virtual void OnVelocity(const std_msgs::Float32::ConstPtr &msg); 61 | 62 | /// \brief Receive detach messages 63 | /// \param[in] msg Boolean detach message. Detach joints if data is 64 | /// true. 65 | private: virtual void OnDetach(const std_msgs::Bool::ConstPtr &msg); 66 | 67 | /// \brief Receive attach messages 68 | /// \param[in] msg Pose attach message. Teleport and attach harness. 69 | private: virtual void OnAttach(const geometry_msgs::Pose::ConstPtr &msg); 70 | 71 | /// \brief Custom callback queue thread 72 | private: void QueueThread(); 73 | 74 | /// \brief pointer to ros node 75 | private: ros::NodeHandle *rosnode_; 76 | 77 | /// \brief Subscriber to velocity control messages. 78 | private: ros::Subscriber velocitySub_; 79 | 80 | /// \brief Subscriber to detach control messages. 81 | private: ros::Subscriber detachSub_; 82 | 83 | /// \brief Subscriber to attach control messages. 84 | private: ros::Subscriber attachSub_; 85 | 86 | /// \brief for setting ROS name space 87 | private: std::string robotNamespace_; 88 | private: ros::CallbackQueue queue_; 89 | private: boost::thread callbackQueueThread_; 90 | }; 91 | } 92 | #endif 93 | -------------------------------------------------------------------------------- /include/srcsim/SolarPanelPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #ifndef GAZEBO_PLUGINS_SOLARPANELPLUGIN_HH_ 18 | #define GAZEBO_PLUGINS_SOLARPANELPLUGIN_HH_ 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace gazebo 29 | { 30 | class GAZEBO_VISIBLE SolarPanelPlugin : public ModelPlugin 31 | { 32 | /// \brief Constructor 33 | public: SolarPanelPlugin(); 34 | 35 | // Documentation inherited 36 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 37 | 38 | /// \brief Callback for enable "service". 39 | /// \param[in] _msg 0 to stop, 1 to start, 2 to open the panel (cheat). 40 | public: void Enable(ConstIntPtr &_msg); 41 | 42 | /// \brief Update plugin's function. 43 | private: void OnUpdate(const common::UpdateInfo &/*_info*/); 44 | 45 | /// \brief Pointer to the solar panel model. 46 | private: physics::ModelPtr model; 47 | 48 | /// \brief Pointer to the button joint. 49 | private: physics::JointPtr buttonJoint; 50 | 51 | /// \brief Pointer to the joints locking the panel closed. 52 | private: std::vector lockJoints; 53 | 54 | /// \brief Joints which will be actuated to open the panel. 55 | private: std::vector panelJoints; 56 | 57 | /// \brief Whether the button is pressed. 58 | private: bool pressed = false; 59 | 60 | /// \brief Whether a cheat was used to open the panel. 61 | private: bool cheated = false; 62 | 63 | /// \brief Lower limit of the button's joint. 64 | private: double lowerLimit; 65 | 66 | /// \brief Upper limit of the button's joint. 67 | private: double upperLimit; 68 | 69 | /// \brief The range of motion of the button's joint. 70 | private: double range; 71 | 72 | /// \brief Between 0% of the range and this value the button is considered 73 | /// not pressed. From this value to 100% the button is considered pressed. 74 | private: static const int kPercentageButtonPressed = 75; 75 | 76 | /// \brief Contact sensor attached to button 77 | private: sensors::ContactSensorPtr contactSensor; 78 | 79 | /// \brief Gazebo transport node for communication. 80 | private: transport::NodePtr gzNode; 81 | 82 | /// \brief Publisher which publishes a message once the panel is open. 83 | private: transport::PublisherPtr openedPub; 84 | 85 | /// \brief Subscriber to enable messages. 86 | private: transport::SubscriberPtr enableSub; 87 | 88 | /// \brief Connection to World Update events. 89 | private: event::ConnectionPtr updateConnection; 90 | }; 91 | } 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /scripts/Traj_data.yaml: -------------------------------------------------------------------------------- 1 | # the first column is the number of seconds to reach this point, 2 | # relative to the beginning of the trajectory 3 | # the last column is a space-separated string of joint angles 4 | # for the arm and neck trajectories, and roll-pitch-yaw for the head 5 | # for the arms, the second column is a string "left" or "right" 6 | # indicating which arm is being commanded 7 | head0: 8 | - [1.0, "0 0 0"] 9 | headRoll: 10 | - [2.0, " 0.5 0 0"] 11 | - [4.0, "-0.5 0 0"] 12 | - [6.0, " 0 0 0"] 13 | headPitch: 14 | - [2.0, "0 0.5 0"] 15 | - [4.0, "0 -0.5 0"] 16 | - [6.0, "0 0 0"] 17 | headYaw: 18 | - [2.0, "0 0 1.0"] 19 | - [4.0, "0 0 -1.0"] 20 | - [6.0, "0 0 0"] 21 | headStuck: 22 | - [2.0, " 0 -0.5 1.0"] 23 | - [4.0, " 0.1 -0.5 1.0"] 24 | - [6.0, "-0.1 -0.5 1.0"] 25 | - [8.0, "0 0 0"] 26 | neck0: 27 | - [1.0, "0 0 0"] 28 | neck1: 29 | - [2.0, "0.5 0 0"] 30 | - [4.0, "0 0 0"] 31 | neck2: 32 | - [2.0, "0 1.0 0"] 33 | - [4.0, "0 -1.0 0"] 34 | - [6.0, "0 0 0"] 35 | neck3: 36 | - [2.0, "0 0 -0.5"] 37 | - [4.0, "0 0 0"] 38 | pigeon: 39 | - [2.0, "0.5 0 -0.5"] 40 | - [4.0, "0 0 0"] 41 | zero: 42 | - [2.0, "left", "0 0 0 0 0 0 0"] 43 | - [2.0, "right", "0 0 0 0 0 0 0"] 44 | arms0: 45 | - [2.0, "left", "0 0 0 0 0 0 0"] 46 | - [2.0, "right", "0 0 0 0 0 0 0"] 47 | arms1: 48 | - [2.0, "left", " 1 0 0 0 0 0 0"] 49 | - [2.0, "right", " 1 0 0 0 0 0 0"] 50 | - [4.0, "left", "-1 0 0 0 0 0 0"] 51 | - [4.0, "right", "-1 0 0 0 0 0 0"] 52 | - [6.0, "left", " 0 0 0 0 0 0 0"] 53 | - [6.0, "right", " 0 0 0 0 0 0 0"] 54 | arms2: 55 | - [2.0, "left", "0 1 0 0 0 0 0"] 56 | - [2.0, "right", "0 1 0 0 0 0 0"] 57 | - [4.0, "left", "0 -1 0 0 0 0 0"] 58 | - [4.0, "right", "0 -1 0 0 0 0 0"] 59 | - [6.0, "left", "0 0 0 0 0 0 0"] 60 | - [6.0, "right", "0 0 0 0 0 0 0"] 61 | arms3: 62 | - [2.0, "left", "0 0 1 0 0 0 0"] 63 | - [2.0, "right", "0 0 1 0 0 0 0"] 64 | - [4.0, "left", "0 0 -1 0 0 0 0"] 65 | - [4.0, "right", "0 0 -1 0 0 0 0"] 66 | - [6.0, "left", "0 0 0 0 0 0 0"] 67 | - [6.0, "right", "0 0 0 0 0 0 0"] 68 | arms4: 69 | - [2.0, "left", "0 0 0 -1 0 0 0"] 70 | - [2.0, "right", "0 0 0 1 0 0 0"] 71 | - [4.0, "left", "0 0 0 0.1 0 0 0"] 72 | - [4.0, "right", "0 0 0 -0.1 0 0 0"] 73 | - [6.0, "left", "0 0 0 0 0 0 0"] 74 | - [6.0, "right", "0 0 0 0 0 0 0"] 75 | arms5: 76 | - [2.0, "left", "0 0 0 0 1 0 0"] 77 | - [2.0, "right", "0 0 0 0 1 0 0"] 78 | - [4.0, "left", "0 0 0 0 -1 0 0"] 79 | - [4.0, "right", "0 0 0 0 -1 0 0"] 80 | - [6.0, "left", "0 0 0 0 0 0 0"] 81 | - [6.0, "right", "0 0 0 0 0 0 0"] 82 | arms6: 83 | - [2.0, "left", "0 0 0 0 0 0.6 0"] 84 | - [2.0, "right", "0 0 0 0 0 0.6 0"] 85 | - [4.0, "left", "0 0 0 0 0 -0.6 0"] 86 | - [4.0, "right", "0 0 0 0 0 -0.6 0"] 87 | - [6.0, "left", "0 0 0 0 0 0 0"] 88 | - [6.0, "right", "0 0 0 0 0 0 0"] 89 | arms7: 90 | - [2.0, "left", "0 0 0 0 0 0 0.4"] 91 | - [2.0, "right", "0 0 0 0 0 0 0.3"] 92 | - [4.0, "left", "0 0 0 0 0 0 -0.3"] 93 | - [4.0, "right", "0 0 0 0 0 0 -0.4"] 94 | - [6.0, "left", "0 0 0 0 0 0 0"] 95 | - [6.0, "right", "0 0 0 0 0 0 0"] 96 | armDemo1: 97 | - [2.0, "left", "0 0 0 0 0 0 0"] 98 | - [4.0, "left", "0.1 -1.3 1.94 -1.18 0.0 -0.07 0.0"] 99 | - [2.0, "right", "0 0 0 0 0 0 0"] 100 | - [3.0, "right", "0 0 0 1 0 0 0"] 101 | - [4.0, "right", "0 0 0 0 0 0 0"] 102 | - [6.0, "right", "-0.1 1.3 1.94 1.18 0.0 0.07 0"] 103 | -------------------------------------------------------------------------------- /include/srcsim/SatellitePlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | 28 | namespace gazebo 29 | { 30 | class SatelliteDishPlugin : public ModelPlugin 31 | { 32 | /// \brief Constructor 33 | public: SatelliteDishPlugin(); 34 | 35 | // Documentation inherited 36 | public: void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 37 | 38 | /// \brief Callback for enable "service". 39 | /// \param[in] _msg 0 to stop, 1 to start 40 | public: void Enable(ConstIntPtr &_msg); 41 | 42 | /// \brief Called on every world update. 43 | /// \param[in] _info Info about the world. 44 | public: void OnUpdate(const common::UpdateInfo &_info); 45 | 46 | /// \brief Ratio between wheel and dish yaw. 47 | private: double yawRatio; 48 | 49 | /// \brief Ratio between wheel and dish pitch. 50 | private: double pitchRatio; 51 | 52 | /// \brief Target yaw angle in radians. 53 | private: double yawTarget; 54 | 55 | /// \brief Target pitch angle in radians. 56 | private: double pitchTarget; 57 | 58 | /// \brief Publish frequecy in Hz. 59 | private: double frequency; 60 | 61 | /// \brief Angle tolerance in degrees. 62 | private: double tolerance; 63 | 64 | /// \brief Topic for satellite messages. 65 | private: std::string satTopic; 66 | 67 | /// \brief Pointer to the yaw wheel joint. 68 | private: physics::JointPtr yawWheel; 69 | 70 | /// \brief Pointer to the pitch wheel joint. 71 | private: physics::JointPtr pitchWheel; 72 | 73 | /// \brief Pointer to the yaw dish joint. 74 | private: physics::JointPtr yawJoint; 75 | 76 | /// \brief Pointer to the pitch dish joint. 77 | private: physics::JointPtr pitchJoint; 78 | 79 | /// \brief Pointer to the joint controller. 80 | private: physics::JointControllerPtr controller; 81 | 82 | /// \brief Time to hold the correct angles. 83 | private: common::Time targetTime; 84 | 85 | /// \brief Time when yaw satrted being correct. 86 | private: common::Time yawCorrectStart; 87 | 88 | /// \brief Time when pitch satrted being correct. 89 | private: common::Time pitchCorrectStart; 90 | 91 | /// \brief Ros node handle 92 | private: std::unique_ptr rosNode; 93 | 94 | /// \brief Ros satellite publisher 95 | private: ros::Publisher satelliteRosPub; 96 | 97 | /// \brief Gazebo transport node for communication. 98 | private: transport::NodePtr gzNode; 99 | 100 | /// \brief Subscriber to enable messages. 101 | private: transport::SubscriberPtr enableSub; 102 | 103 | /// \brief List of connections 104 | private: std::vector connections; 105 | }; 106 | GZ_REGISTER_MODEL_PLUGIN(SatelliteDishPlugin) 107 | } 108 | 109 | -------------------------------------------------------------------------------- /worlds/air_leak_detector.erb: -------------------------------------------------------------------------------- 1 | <% 2 | # Template for air leak detector model for SRC finals world 3 | 4 | # Requires the following global variables: 5 | # $air_leak_detector_frame, $table_top_z 6 | 7 | # Print inertial element based on a box 8 | def printInertialBox(_com_x, _com_y, _com_z, _mass, _len_x, _len_y, _len_z) 9 | "\n"\ 10 | " \n"\ 11 | " " + _com_x.to_s() + "\n"\ 12 | " " + _com_y.to_s() + "\n"\ 13 | " " + _com_z.to_s() + "\n"\ 14 | " " + 0.to_s() + "\n"\ 15 | " " + 0.to_s() + "\n"\ 16 | " " + 0.to_s() + "\n"\ 17 | " \n"\ 18 | " \n"\ 19 | " " + (_mass / 12 * (_len_y*_len_y + _len_z*_len_z)).to_s() + "\n"\ 20 | " " + (_mass / 12 * (_len_x*_len_x + _len_z*_len_z)).to_s() + "\n"\ 21 | " " + (_mass / 12 * (_len_x*_len_x + _len_y*_len_y)).to_s() + "\n"\ 22 | " \n"\ 23 | " " + _mass.to_s() + "\n"\ 24 | " \n"\ 25 | end 26 | 27 | # Scale 28 | scale = 0.02 29 | 30 | # Masse in kg 31 | mass = 1.0 32 | 33 | # CoM offset 34 | com_x = 0 35 | com_y = -0.0315 * scale 36 | com_z = 1.1 * scale 37 | 38 | # Inertia box dimensions 39 | inertia_x = 9.437 * scale 40 | inertia_y = 3.311 * scale 41 | inertia_z = 2.673 * scale 42 | 43 | offset_z = 4.724 * scale 44 | %> 45 | 46 | 47 | 48 | 49 | <%= $air_leak_detector_frame[0, 2] %> 50 | <%= $air_leak_detector_frame[1, 2] %> 51 | <%= $table_top_z + offset_z %> 52 | 0 53 | 0 54 | <%= Math::atan2($air_leak_detector_frame[1, 0], $air_leak_detector_frame[0, 0]) + Math::PI * 0.5 %> 55 | 56 | 57 | 58 | 59 | <%= printInertialBox(com_x, com_y, 0, mass, inertia_x, inertia_y, inertia_z) %> 60 | 61 | 62 | 63 | 0 0 <%= -com_z %> 0 0 <%= -Math::PI * 0.5 %> 64 | 65 | 66 | 67 | <%= scale %> <%= scale %> <%= scale %> 68 | model://air_leak_detector/meshes/air_leak_detector.obj 69 | 70 | 71 | 72 | 73 | 74 | 1e5 75 | 1 76 | 0.1 77 | 0.0015 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 0 0 <%= -com_z %> 0 0 <%= -Math::PI * 0.5 %> 86 | 87 | 88 | 89 | <%= scale %> <%= scale %> <%= scale %> 90 | model://air_leak_detector/meshes/air_leak_detector.obj 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 0.2 99 | 0.5 100 | <%= Math::PI / 9 %> 101 | 1 102 | 103 | 104 | true 105 | true 106 | 10 107 | 108 | 109 | 110 | 111 | 112 | collision 113 | 114 | 115 | 116 | 117 | 118 | 119 | contact_sensor 120 | valkyrie 121 | 122 | task3/checkpoint4 123 | true 124 | 125 | 126 | 127 | 128 | -------------------------------------------------------------------------------- /src/Qual2Plugin.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | #include "srcsim/Qual2Plugin.hh" 26 | 27 | using namespace gazebo; 28 | 29 | GZ_REGISTER_MODEL_PLUGIN(Qual2Plugin) 30 | 31 | ///////////////////////////////////////////////// 32 | Qual2Plugin::Qual2Plugin() 33 | { 34 | } 35 | 36 | ///////////////////////////////////////////////// 37 | void Qual2Plugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) 38 | { 39 | GZ_ASSERT(_model, "Model pointer is null"); 40 | GZ_ASSERT(_sdf, "SDF pointer is null"); 41 | 42 | // Read the hinge joint name. 43 | if (!_sdf->HasElement("hinge_joint_name")) 44 | { 45 | gzerr << " not specified in SDF\n"; 46 | return; 47 | } 48 | auto hingeJointName = _sdf->Get("hinge_joint_name"); 49 | this->hingeJoint = _model->GetJoint(hingeJointName); 50 | if (!this->hingeJoint) 51 | { 52 | gzerr << "Joint [" << hingeJointName << "] not found" << std::endl; 53 | return; 54 | } 55 | 56 | // Read the button joint name. 57 | if (!_sdf->HasElement("button_joint_name")) 58 | { 59 | gzerr << " not specified in SDF\n"; 60 | return; 61 | } 62 | auto buttonJointName = _sdf->Get("button_joint_name"); 63 | this->buttonJoint = _model->GetJoint(buttonJointName); 64 | if (!this->buttonJoint) 65 | { 66 | gzerr << "Joint [" << buttonJointName << "] not found" << std::endl; 67 | return; 68 | } 69 | 70 | this->lowerLimit = this->buttonJoint->GetLowerLimit(0).Radian(); 71 | this->upperLimit = this->buttonJoint->GetUpperLimit(0).Radian(); 72 | this->range = this->upperLimit - this->lowerLimit; 73 | 74 | this->updateConnection = event::Events::ConnectWorldUpdateBegin( 75 | std::bind(&Qual2Plugin::OnUpdate, this)); 76 | } 77 | 78 | ///////////////////////////////////////////////// 79 | bool Qual2Plugin::Pressed() const 80 | { 81 | return this->pressed; 82 | } 83 | 84 | //////////////////////////////////////////////// 85 | void Qual2Plugin::OpenTheDoor() 86 | { 87 | // Apply force on the hinge to open the door. 88 | this->hingeJoint->SetForce(0, 400.0); 89 | } 90 | 91 | //////////////////////////////////////////////// 92 | void Qual2Plugin::CloseTheDoor() 93 | { 94 | // Apply force on the hinge to close the door. 95 | this->hingeJoint->SetForce(0, -400.0); 96 | } 97 | 98 | 99 | ///////////////////////////////////////////////// 100 | void Qual2Plugin::OnUpdate() 101 | { 102 | // Something went wrong, nothing to do. 103 | if (!this->buttonJoint || !this->hingeJoint) 104 | return; 105 | 106 | int percentagePressed = 107 | ((this->buttonJoint->GetAngle(0).Radian() - this->lowerLimit) / 108 | this->range) * 100; 109 | 110 | this->pressed = percentagePressed >= this->kPercentageButtonPressed; 111 | 112 | // Trigger the opening of the door. 113 | if (this->pressed) 114 | this->opening = true; 115 | 116 | // Open/close the door. 117 | if (this->opening) 118 | this->OpenTheDoor(); 119 | else 120 | this->CloseTheDoor(); 121 | } 122 | -------------------------------------------------------------------------------- /include/srcsim/BoxContainsPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | namespace gazebo 25 | { 26 | /// \brief Plugin which emits gazebo transport messages according to whether 27 | /// an entity is inside or outside an oriented box. 28 | /// 29 | /// Example usage: 30 | /// 31 | /// 32 | /// 33 | /// 35 | /// true 36 | /// 37 | /// 38 | /// robot 39 | /// 40 | /// 45 | /// gazebo/robot 46 | /// 47 | /// 48 | /// 1 1 4 49 | /// 50 | /// 51 | /// 10 10 2 0 0 1.57 52 | /// 53 | /// 54 | /// 55 | class BoxContainsPlugin : public WorldPlugin 56 | { 57 | // Documentation inherited 58 | public: BoxContainsPlugin(); 59 | 60 | // Documentation inherited 61 | public: void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf); 62 | 63 | /// \brief Called every world iteration on world update begin. 64 | /// \param[in] _info Update info. 65 | private: void OnUpdate(const common::UpdateInfo &_info); 66 | 67 | /// \brief Callback for toggle "service". 68 | /// \param[in] _msg Unused message 69 | public: void Toggle(ConstIntPtr &/*_msg*/); 70 | 71 | /// \brief Connection to world update. 72 | private: event::ConnectionPtr updateConnection; 73 | 74 | /// \brief Pointer to the world. 75 | private: physics::WorldPtr world; 76 | 77 | /// \brief Scoped name of the entity we're checking. 78 | private: std::string entityName; 79 | 80 | /// \brief Pointer to the entity we're checking. 81 | private: physics::EntityPtr entity; 82 | 83 | /// \brief Box representing the volume to check. 84 | private: ignition::math::OrientedBoxd box; 85 | 86 | /// \brief Gazebo transport node for communication. 87 | private: transport::NodePtr gzNode; 88 | 89 | /// \brief Publisher which publishes contain / doesn't contain messages. 90 | private: transport::PublisherPtr containsPub; 91 | 92 | /// \brief Subscriber to toggle messages. 93 | private: transport::SubscriberPtr toggleSub; 94 | 95 | /// \brief Namespace for the topics: 96 | /// //box/contains 97 | /// //box/toggle 98 | private: std::string ns; 99 | 100 | /// \brief Whether contains or not 101 | public: int contains = -1; 102 | 103 | /// \brief Previous update time. 104 | public: common::Time prevTime; 105 | }; 106 | } 107 | 108 | -------------------------------------------------------------------------------- /include/srcsim/Task1.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef SRC_TASK1_HH_ 19 | #define SRC_TASK1_HH_ 20 | 21 | #include 22 | #include 23 | #include 24 | 25 | #include 26 | #include "Checkpoint.hh" 27 | #include "Task.hh" 28 | 29 | namespace gazebo 30 | { 31 | class Task1 : public Task 32 | { 33 | /// \brief Constructor 34 | /// \param[in] _sdf Pointer to SDF element for this task. 35 | public: Task1(const sdf::ElementPtr &_sdf); 36 | 37 | // Documentation inherited 38 | public: size_t Number() const; 39 | }; 40 | 41 | /// \brief Task 1, Checkpoint 1: Walk to dish 42 | class Task1CP1 : public BoxCheckpoint 43 | { 44 | using BoxCheckpoint::BoxCheckpoint; 45 | 46 | /// \brief Check whether the robot is currently within the box volume in 47 | /// front of the satellite dish. 48 | public: bool Check(); 49 | 50 | /// \brief Reharness at start box 51 | /// \param[in] _penalty Penalty time to add 52 | public: void Restart(const common::Time &_penalty); 53 | }; 54 | 55 | /// \brief Task 1, Checkpoint 2: Satellite pitch or yaw 56 | class Task1CP2 : public Checkpoint 57 | { 58 | using Checkpoint::Checkpoint; 59 | 60 | /// \brief Check whether the last message received from the satellite had 61 | /// the checkpoint complete. 62 | /// \return True if the checkpoint is compelete. 63 | public: bool Check(); 64 | 65 | /// \brief Callback when a satellite message is received. 66 | /// \param[in] _msg Satellite message 67 | private: void OnSatelliteRosMsg(const srcsim::Satellite &_msg); 68 | 69 | /// \brief Whether the checkpoint is complete or not. 70 | private: bool oneAxisDone = false; 71 | 72 | /// \brief Ros node handle 73 | private: std::unique_ptr rosNode; 74 | 75 | /// \brief Gazebo transport node for communication. 76 | private: transport::NodePtr gzNode; 77 | 78 | /// \brief Subscribes to ROS satellite messages. 79 | private: ros::Subscriber satelliteRosSub; 80 | }; 81 | 82 | /// \brief Task 1, Checkpoint 3: Satellite pitch and yaw 83 | class Task1CP3 : public Checkpoint 84 | { 85 | using Checkpoint::Checkpoint; 86 | 87 | /// \brief Check whether the last message received from the satellite had 88 | /// the checkpoint complete. 89 | /// \return True if the checkpoint is compelete. 90 | public: bool Check(); 91 | 92 | /// \brief Callback when a satellite message is received. 93 | /// \param[in] _msg Satellite message 94 | private: void OnSatelliteRosMsg(const srcsim::Satellite &_msg); 95 | 96 | /// \brief Whether the checkpoint is complete or not. 97 | private: bool satelliteDone = false; 98 | 99 | /// \brief Ros node handle 100 | private: std::unique_ptr rosNode; 101 | 102 | /// \brief Gazebo transport node for communication. 103 | private: transport::NodePtr gzNode; 104 | 105 | /// \brief Subscribes to ROS satellite messages. 106 | private: ros::Subscriber satelliteRosSub; 107 | }; 108 | 109 | /// \brief Task 1, Checkpoint 4: Final box 110 | class Task1CP4 : public BoxCheckpoint 111 | { 112 | using BoxCheckpoint::BoxCheckpoint; 113 | 114 | /// \brief Check whether the robot is in the final box region. 115 | /// \return True if the checkpoint is compelete. 116 | public: bool Check(); 117 | }; 118 | } 119 | #endif 120 | -------------------------------------------------------------------------------- /src/BoxContainsPlugin.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include 19 | 20 | #include 21 | #include 22 | 23 | #include "srcsim/BoxContainsPlugin.hh" 24 | 25 | using namespace gazebo; 26 | 27 | GZ_REGISTER_WORLD_PLUGIN(BoxContainsPlugin) 28 | 29 | ///////////////////////////////////////////////// 30 | BoxContainsPlugin::BoxContainsPlugin() : WorldPlugin() 31 | { 32 | } 33 | 34 | ///////////////////////////////////////////////// 35 | void BoxContainsPlugin::Load(physics::WorldPtr _world, sdf::ElementPtr _sdf) 36 | { 37 | // Load SDF params 38 | if (!_sdf->HasElement("size")) 39 | { 40 | gzerr << "Missing required parameter " << std::endl; 41 | return; 42 | } 43 | 44 | if (!_sdf->HasElement("pose")) 45 | { 46 | gzerr << "Missing required parameter " << std::endl; 47 | return; 48 | } 49 | 50 | if (!_sdf->HasElement("entity")) 51 | { 52 | gzerr << "Missing required parameter " << std::endl; 53 | return; 54 | } 55 | 56 | if (!_sdf->HasElement("namespace")) 57 | { 58 | gzerr << "Missing required parameter " << std::endl; 59 | return; 60 | } 61 | 62 | auto size = _sdf->Get("size"); 63 | auto pose = _sdf->Get("pose"); 64 | this->entityName = _sdf->Get("entity"); 65 | this->ns = _sdf->Get("namespace"); 66 | 67 | this->box = ignition::math::OrientedBoxd(size, pose); 68 | 69 | this->world = _world; 70 | 71 | // Start/stop "service" 72 | this->gzNode = transport::NodePtr(new transport::Node()); 73 | this->gzNode->Init(); 74 | this->toggleSub = this->gzNode->Subscribe("/" + this->ns + "/box/toggle", 75 | &BoxContainsPlugin::Toggle, this); 76 | 77 | auto enabled = _sdf->HasElement("enabled") && _sdf->Get("enabled"); 78 | if (enabled) 79 | { 80 | ConstIntPtr msg; 81 | this->Toggle(msg); 82 | } 83 | } 84 | 85 | ////////////////////////////////////////////////// 86 | void BoxContainsPlugin::Toggle(ConstIntPtr &/*_msg*/) 87 | { 88 | // Start 89 | if (!this->updateConnection) 90 | { 91 | this->entity = this->world->GetEntity(this->entityName); 92 | if (!this->entity) 93 | { 94 | gzerr << "Can't find entity[" << entity << 95 | "] in world. Failed to enable Box Plugin." << std::endl; 96 | return; 97 | } 98 | 99 | // Start update 100 | this->updateConnection = event::Events::ConnectWorldUpdateBegin( 101 | std::bind(&BoxContainsPlugin::OnUpdate, this, std::placeholders::_1)); 102 | 103 | this->containsPub = this->gzNode->Advertise( 104 | "/" + this->ns + "/box/contains"); 105 | gzmsg << "Started box contains plugin [" << this->ns << "]" << std::endl; 106 | } 107 | // Stop 108 | else 109 | { 110 | this->updateConnection.reset(); 111 | gzmsg << "Stopped box contains plugin [" << this->ns << "]" << std::endl; 112 | } 113 | } 114 | 115 | ///////////////////////////////////////////////// 116 | void BoxContainsPlugin::OnUpdate(const common::UpdateInfo &/*_info*/) 117 | { 118 | // For safety 119 | if (!this->entity) 120 | { 121 | gzerr << "Entity is null" << std::endl; 122 | return; 123 | } 124 | 125 | auto pos = this->entity->GetWorldPose().Ign().Pos(); 126 | auto containsNow = this->box.Contains(pos) ? 1 : 0; 127 | 128 | if (containsNow != this->contains) 129 | { 130 | this->contains = containsNow; 131 | 132 | msgs::Int msg; 133 | msg.set_data(this->contains); 134 | 135 | this->containsPub->Publish(msg); 136 | } 137 | } 138 | 139 | -------------------------------------------------------------------------------- /src/srcsim_ros_harness.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | #include "srcsim/srcsim_ros_harness.h" 23 | 24 | namespace gazebo 25 | { 26 | 27 | // Register this plugin with the simulator 28 | GZ_REGISTER_MODEL_PLUGIN(SRCSimRosHarness) 29 | 30 | ///////////////////////////////////////////////// 31 | SRCSimRosHarness::SRCSimRosHarness() 32 | { 33 | } 34 | 35 | ///////////////////////////////////////////////// 36 | SRCSimRosHarness::~SRCSimRosHarness() 37 | { 38 | // Custom Callback Queue 39 | this->queue_.clear(); 40 | this->queue_.disable(); 41 | 42 | this->rosnode_->shutdown(); 43 | delete this->rosnode_; 44 | } 45 | 46 | ///////////////////////////////////////////////// 47 | // Load the controller 48 | void SRCSimRosHarness::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) 49 | { 50 | // Load the plugin 51 | SRCHarnessPlugin::Load(_parent, _sdf); 52 | 53 | this->robotNamespace_ = ""; 54 | if (_sdf->HasElement("robotNamespace")) 55 | this->robotNamespace_ = _sdf->Get("robotNamespace") + "/"; 56 | 57 | // Init ROS 58 | if (!ros::isInitialized()) 59 | { 60 | ROS_FATAL_STREAM("Not loading plugin since ROS hasn't been " 61 | << "properly initialized. Try starting gazebo with ros plugin:\n" 62 | << " gazebo -s libgazebo_ros_api_plugin.so\n"); 63 | return; 64 | } 65 | 66 | this->rosnode_ = new ros::NodeHandle(this->robotNamespace_); 67 | 68 | ros::SubscribeOptions so = ros::SubscribeOptions::create( 69 | "/" + _parent->GetName() + "/harness/velocity", 1, 70 | boost::bind(&SRCSimRosHarness::OnVelocity, this, _1), 71 | ros::VoidPtr(), &this->queue_); 72 | this->velocitySub_ = this->rosnode_->subscribe(so); 73 | 74 | so = ros::SubscribeOptions::create( 75 | "/" + _parent->GetName() + "/harness/detach", 1, 76 | boost::bind(&SRCSimRosHarness::OnDetach, this, _1), 77 | ros::VoidPtr(), &this->queue_); 78 | this->detachSub_ = this->rosnode_->subscribe(so); 79 | 80 | so = ros::SubscribeOptions::create( 81 | "/" + _parent->GetName() + "/harness/attach", 1, 82 | boost::bind(&SRCSimRosHarness::OnAttach, this, _1), 83 | ros::VoidPtr(), &this->queue_); 84 | this->attachSub_ = this->rosnode_->subscribe(so); 85 | 86 | // Custom Callback Queue 87 | this->callbackQueueThread_ = 88 | boost::thread(boost::bind(&SRCSimRosHarness::QueueThread, this)); 89 | } 90 | 91 | ///////////////////////////////////////////////// 92 | void SRCSimRosHarness::OnVelocity(const std_msgs::Float32::ConstPtr &msg) 93 | { 94 | // Set the target winch velocity 95 | this->SetWinchVelocity(msg->data); 96 | } 97 | 98 | ///////////////////////////////////////////////// 99 | void SRCSimRosHarness::OnDetach(const std_msgs::Bool::ConstPtr &msg) 100 | { 101 | // Detach if true 102 | if (msg->data) 103 | this->Detach(); 104 | } 105 | 106 | ///////////////////////////////////////////////// 107 | void SRCSimRosHarness::OnAttach(const geometry_msgs::Pose::ConstPtr &msg) 108 | { 109 | ignition::math::Vector3d pos( 110 | msg->position.x, 111 | msg->position.y, 112 | msg->position.z); 113 | ignition::math::Quaterniond rot( 114 | msg->orientation.w, 115 | msg->orientation.x, 116 | msg->orientation.y, 117 | msg->orientation.z); 118 | this->Attach(ignition::math::Pose3d(pos, rot)); 119 | } 120 | 121 | ///////////////////////////////////////////////// 122 | void SRCSimRosHarness::QueueThread() 123 | { 124 | static const double timeout = 0.01; 125 | 126 | while (this->rosnode_->ok()) 127 | { 128 | this->queue_.callAvailable(ros::WallDuration(timeout)); 129 | } 130 | } 131 | } 132 | -------------------------------------------------------------------------------- /cmake/setup.bash.in: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | ROS_DISTRO="indigo" 4 | LINUX_GROUP_NEEDED="ros" 5 | JAVA_HOME_DIR="/usr/lib/jvm/java-8-openjdk-amd64" 6 | LIMITD_FILE="/etc/security/limits.d/ros-rtprio.conf" 7 | NASA_SETUP_FILE="/opt/nasa/${ROS_DISTRO}/setup.bash" 8 | # modify the URI to point to default? 9 | IHMC_NETWORK_FILE_URI="https://raw.githubusercontent.com/ihmcrobotics/ihmc_ros_core/0.8.0/ihmc_ros_common/configurations/IHMCNetworkParametersTemplate.ini" 10 | IHMC_NETWORK_FILE="${HOME}/.ihmc/IHMCNetworkParameters.ini" 11 | IHMC_JAVA_ADAPTER_DIR="/opt/ros/${ROS_DISTRO}/share/ihmc_ros_java_adapter" 12 | 13 | get_current_update_alternatives_link() 14 | { 15 | local software=${1} 16 | 17 | echo "$(update-alternatives --query ${software} | grep Value | awk '{ print $2 }')" 18 | } 19 | 20 | check_java_according_to_java_home() 21 | { 22 | local software=${1} 23 | 24 | current_java_link=$(get_current_update_alternatives_link ${software}) 25 | 26 | # Check if the value does not contain the JAVA_HOME_DIR 27 | if [[ "${current_java_link/${JAVA_HOME_DIR}}" == "${current_java_link}" ]]; then 28 | return 1 29 | fi 30 | 31 | return 0 32 | } 33 | 34 | # Source the NASA setup.bash file 35 | if [[ ! -f ${NASA_SETUP_FILE} ]]; then 36 | echo "The file ${NASA_SETUP_FILE} was not found." 37 | echo "Is the nasa-${ROS_DISTRO}-workspace installed?" 38 | return 1 39 | fi 40 | 41 | source ${NASA_SETUP_FILE} 42 | 43 | # Point JAVA_HOME to openjdk8-jdk 44 | if [[ ! -d ${JAVA_HOME_DIR} ]]; then 45 | echo "The directory ${JAVA_HOME_DIR} was not found." 46 | echo "Is the openjdk-8-jdk-headless package installed?" 47 | return 1 48 | fi 49 | 50 | export JAVA_HOME=${JAVA_HOME_LINK} 51 | 52 | # Download the IHMCNetworkParametersTemplate.ini file if needed 53 | if [[ ! -f ${IHMC_NETWORK_FILE} ]]; then 54 | echo " * IHMCNetworkParameters.ini not found, download it now to ${IHMC_NETWORK_FILE}" 55 | mkdir -p ${HOME}/.ihmc 56 | curl ${IHMC_NETWORK_FILE_URI} > ${IHMC_NETWORK_FILE} 2>/dev/null 57 | fi 58 | 59 | export IS_GAZEBO=trueexport 60 | export ROS_IP=127.0.0.1 61 | 62 | # Check java links 63 | if ! $(check_java_according_to_java_home java); then 64 | echo "update-alternatives report a not valid path java8 path:" 65 | echo "$(get_current_update_alternatives_link java)" 66 | return 1 67 | fi 68 | 69 | if ! $(check_java_according_to_java_home javac); then 70 | echo "update-alternatives report a not valid path javac8 path:" 71 | echo "$(get_current_update_alternatives_link javac)" 72 | return 1 73 | fi 74 | 75 | # Check perms on ihmc_java_adapter 76 | if [[ ! -w ${IHMC_JAVA_ADAPTER_DIR} ]]; then 77 | echo "The user ${USER} does not have write permissions on ${IHMC_JAVA_ADAPTER_DIR}" 78 | echo "If the system is not intended to run srcsim for different system user" 79 | echo "you can run 'sudo chown -R $USER:$USER /opt/ros/indigo/share/ihmc_ros_java_adapter'" 80 | echo "to allow the user to write on it" 81 | return 1 82 | fi 83 | 84 | # Check ros groups exists and user belongs to it 85 | if [[ ! $(getent group ${LINUX_GROUP_NEEDED}) && 86 | ! $(groups ${USER} | grep -q ${LINUX_GROUP_NEEDED}) ]]; then 87 | echo "The user ${USER} does not belong to the group ${LINUX_GROUP_NEEDED}" 88 | echo "To add the group and the user to it:" 89 | echo " - sudo groupadd ${LINUX_GROUP_NEEDED}" 90 | echo " - sudo usermod -a -G ${LINUX_GROUP_NEEDED} $USER" 91 | return 1 92 | fi 93 | 94 | # Check limits.d file 95 | if [[ ! -f ${LIMITD_FILE} ]]; then 96 | echo "The ${LIMITD_FILE} is not present in the system" 97 | echo "To add it with the right configuration:" 98 | echo " - sudo bash -c 'echo \"@${LINUX_GROUP_NEEDED} - rtprio 99\" > /etc/security/limits.d/ros-rtprio.conf'" 99 | return 1 100 | fi 101 | 102 | # Check gazebo models 103 | if [[ ! -d "${HOME}/.gazebo/models" ]]; then 104 | echo "Gazebo models were not found in the user HOME" 105 | echo "To download them:" 106 | echo " - mkdir -p ${HOME}/.gazebo/models" 107 | echo " - wget -qO- https://bitbucket.org/osrf/gazebo_models/get/default.tar.gz | tar xvz --strip 1 -C ${HOME}/.gazebo/models" 108 | return 1 109 | fi 110 | 111 | # Check valkyrie controller 112 | if [[ ! -d "${HOME}/valkyrie" ]]; then 113 | echo "Valkyrie controller was not found in the user HOME" 114 | echo "To download it:" 115 | echo " - mkdir -p ${HOME}/.gazebo/models" 116 | echo " - wget -qO- http://gazebosim.org/distributions/srcsim/valkyrie_controller.tar.gz | tar xvz -C ${HOME}" 117 | return 1 118 | fi 119 | -------------------------------------------------------------------------------- /include/srcsim/Qual1Plugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #ifndef GAZEBO_PLUGINS_WINDPLUGIN_HH_ 18 | #define GAZEBO_PLUGINS_WINDPLUGIN_HH_ 19 | 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | #include 26 | #include "srcsim/Console.h" 27 | 28 | #include "gazebo/common/Plugin.hh" 29 | #include "gazebo/physics/PhysicsTypes.hh" 30 | #include "gazebo/msgs/msgs.hh" 31 | #include "gazebo/transport/TransportTypes.hh" 32 | 33 | namespace gazebo 34 | { 35 | /// \brief Plugin that controls lights for SRC qual task 1. 36 | class GAZEBO_VISIBLE Qual1Plugin : public WorldPlugin 37 | { 38 | /// \brief Constructor 39 | public: Qual1Plugin(); 40 | 41 | /// \brief Load the plugin 42 | /// \param[in] _world Pointer to the world. 43 | /// \param[in] _sdf Pointer to the plugin's sdf element. 44 | public: virtual void Load(physics::WorldPtr _world, sdf::ElementPtr _sdf); 45 | 46 | /// \brief Change the color of a light 47 | /// \param[in] _light Number of the light (1-44) 48 | /// \param[in] _clr Color for the light. 49 | private: void Switch(int _light, const gazebo::common::Color &_clr); 50 | 51 | /// \brief Callback when the competitor has signaled they are ready to 52 | /// start. 53 | /// \param[in] _msg Unused empty message. 54 | private: void OnStart(const std_msgs::EmptyConstPtr &_msg); 55 | 56 | /// \brief Callback when a light answer is received from a competitor 57 | /// \param[in] _msg The position of the light in Val's camera frame. 58 | private: void OnLight(const srcsim::ConsoleConstPtr &_msg); 59 | 60 | /// \brief Callback for World Update events. 61 | private: virtual void OnUpdate(); 62 | 63 | /// \brief Log information 64 | /// \param[in] _string String to output 65 | /// \param[in} _stamp True to output sim time with the _string 66 | private: void Log(const std::string &_string, const bool _stamp); 67 | 68 | /// \brief Gazebo transport node 69 | private: gazebo::transport::NodePtr node; 70 | 71 | /// \brief Gazebo publisher for updating visuals. 72 | private: gazebo::transport::PublisherPtr pub; 73 | 74 | /// \brief Connection to World Update events. 75 | private: event::ConnectionPtr updateConnection; 76 | 77 | /// \brief A class that contains light switching info 78 | private: class LightCtrl 79 | { 80 | /// \brief Console??? 81 | public: int console; 82 | 83 | /// \brief The light index 84 | public: int light; 85 | 86 | /// \brief Delay time for the light change 87 | public: gazebo::common::Time delay; 88 | 89 | /// \brief Color to change the light to when the delay 90 | /// has passed. 91 | public: gazebo::common::Color color; 92 | }; 93 | 94 | /// \brief The pattern of of light changes. 95 | private: std::vector lightPattern; 96 | 97 | /// \brief Iterator to lightPattern 98 | private: std::vector::iterator lightPatternIter; 99 | 100 | /// \brief Time of the last light change 101 | private: gazebo::common::Time prevLightTime; 102 | 103 | /// \brief Pointer to the world. 104 | private: gazebo::physics::WorldPtr world; 105 | 106 | /// \brief Mutex used to prevent interleaved messages. 107 | private: std::mutex mutex; 108 | 109 | /// \brief Log file output stream; 110 | private: std::ofstream logStream; 111 | 112 | /// \brief Ros node handle 113 | private: std::unique_ptr rosnode; 114 | 115 | /// \brief Ros light subscriber 116 | private: ros::Subscriber lightSub; 117 | 118 | /// \brief Ros start subscriber 119 | private: ros::Subscriber startSub; 120 | 121 | private: std::mutex iterMutex; 122 | }; 123 | } 124 | 125 | #endif 126 | -------------------------------------------------------------------------------- /include/srcsim/SRCHarnessPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #ifndef SRC_HARNESSPLUGIN_HH_ 18 | #define SRC_HARNESSPLUGIN_HH_ 19 | 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | namespace gazebo 27 | { 28 | // Forward declare private data class 29 | class SRCHarnessPluginPrivate; 30 | 31 | /// \brief This plugin is designed to lower a model at a controlled rate. 32 | /// Joints between a harness model and a model to lower are created 33 | /// according to SDF provided to this plugin. 34 | /// 35 | /// A winch joint and detach joint can be specified. The winch joint, 36 | /// ideally a prismatic joint, has a PID controller. The detach joint, 37 | /// which can be the same joint as the winch joint, is detached on a given 38 | /// signal. 39 | /// 40 | /// Two topics are created: 41 | /// 42 | /// 1. ~//harness/velocity 43 | /// - Message Type: GzString, expected to be a float 44 | /// - Purpose: Set target winch velocity 45 | /// 46 | /// 2. ~//harness/detach 47 | /// - Message Type: GzString, expected to be a bool ("true") 48 | /// - Purpose: Detach the joint. 49 | /// 50 | /// 3. ~//harness/attach 51 | /// - Message Type: Pose, world pose to be set for child link 52 | /// before attaching 53 | /// - Purpose: Attach the joint at the specified world pose 54 | /// 55 | /// For an example refer to: 56 | /// - World file: worlds/harness.world 57 | /// - Code: examples/stand_alone/harness 58 | class GAZEBO_VISIBLE SRCHarnessPlugin : public ModelPlugin 59 | { 60 | /// \brief Constructor. 61 | public: SRCHarnessPlugin(); 62 | 63 | /// \brief Destructor. 64 | public: virtual ~SRCHarnessPlugin(); 65 | 66 | // Documentation Inherited. 67 | public: virtual void Load(physics::ModelPtr _model, sdf::ElementPtr _sdf); 68 | 69 | // Documentation Inherited. 70 | public: virtual void Init(); 71 | 72 | /// \brief Set the target winch velocity 73 | /// \param[in] _value Target winch velocity. 74 | public: void SetWinchVelocity(const float _value); 75 | 76 | /// \brief Get the current winch velocity 77 | /// \return Velocity of the winch joint 78 | public: double WinchVelocity() const; 79 | 80 | /// \brief Move the child link to the specified pose and recreate the 81 | /// harness joint. 82 | /// \param[in] _pose Desired world pose of child link before harnessing 83 | public: void Attach(const ignition::math::Pose3d &_pose); 84 | 85 | /// \brief Detach the joint. Once the joint is detached, it 86 | /// can be reattached with the Attach method. 87 | public: void Detach(); 88 | 89 | /// \brief Attach harness joints at current robot pose. 90 | /// \param[in] _model Pointer to parent model. 91 | private: void Attach(); 92 | 93 | /// \brief Callback for World Update events. 94 | /// \param[in] _info Update information 95 | private: void OnUpdate(const common::UpdateInfo &_info); 96 | 97 | /// \brief Velocity control callback. 98 | /// \param[in] _msg Message data, interpreted as a float 99 | private: void OnVelocity(ConstGzStringPtr &_msg); 100 | 101 | /// \brief Attach callback. 102 | /// \param[in] _msg Pose where attachment should occur. 103 | private: void OnAttach(ConstPosePtr &_msg); 104 | 105 | /// \brief Detach callback. 106 | /// \param[in] _msg Message data, interpreted as a bool 107 | private: void OnDetach(ConstGzStringPtr &_msg); 108 | 109 | /// \brief Get the index of a joint with the given name. 110 | /// \param[in] _name Name of the joint to find. 111 | /// \return Index into this->jointsto 112 | private: int JointIndex(const std::string &_name) const; 113 | 114 | /// \internal 115 | /// \brief Pointer to private data. 116 | private: std::unique_ptr dataPtr; 117 | }; 118 | } 119 | #endif 120 | -------------------------------------------------------------------------------- /models/leak_patch_tool/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 0 0.02 0.0 0 0 0 9 | 10 | 0.0162 11 | 0.0013 12 | 0.0162 13 | 14 | 2.0 15 | 16 | 17 | 18 | 19 | 20 | 0.04 0.04 0.04 21 | model://leak_patch_tool/meshes/leak_patch_tool.obj 22 | 23 | leak_patch_tool 24 | 25 | 26 | 27 | 28 | 29 | 30 | 1e5 31 | 1 32 | 0.1 33 | 0.0015 34 | 35 | 36 | 37 | 38 | 39 | 40 | 0 0.242 0 1.57 0 0 41 | 42 | 43 | 0.01 44 | 0.008 45 | 46 | 47 | 48 | 49 | 50 | 1e5 51 | 1 52 | 0.1 53 | 0.0015 54 | 55 | 56 | 0x10000 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 0.04 0.04 0.04 65 | model://leak_patch_tool/meshes/leak_patch_tool.obj 66 | 67 | leak_patch_tool 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | collision 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 0 0.114 0.0356 0 0 0 85 | 86 | 0.0000257 87 | 0.000025187 88 | 0.000040125 89 | 90 | 0.1 91 | 92 | 93 | 94 | 95 | 96 | 0.04 0.04 0.04 97 | model://leak_patch_tool/meshes/leak_patch_tool.obj 98 | 99 | leak_patch_tool_button 100 | 101 | 102 | 103 | 104 | 105 | 106 | 1e5 107 | 1 108 | 0.1 109 | 0.0015 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 0.04 0.04 0.04 119 | model://leak_patch_tool/meshes/leak_patch_tool.obj 120 | 121 | leak_patch_tool_button 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | collision 130 | 131 | 132 | 133 | 134 | 135 | 136 | 0 0 0 0 0 0 137 | tool 138 | button 139 | 140 | 0 0 1 141 | 142 | -0.005 143 | 0 144 | 145 | 146 | 10 147 | 1000 148 | 0 149 | 150 | 151 | 152 | 153 | 154 | tool_contact_sensor 155 | button_contact_sensor 156 | valkyrie 157 | 158 | task3/checkpoint6 159 | true 160 | 161 | 162 | 163 | 164 | -------------------------------------------------------------------------------- /include/srcsim/HarnessManager.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef SRC_HARNESSMANAGER_HH_ 19 | #define SRC_HARNESSMANAGER_HH_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | namespace gazebo 28 | { 29 | /// \brief Manages harness 30 | class HarnessManager : public SingletonT 31 | { 32 | /// \brief Constructor 33 | private: HarnessManager(); 34 | 35 | /// \brief Destructor 36 | private: virtual ~HarnessManager(); 37 | 38 | /// \brief Add a new goal, overriding previous one 39 | /// \brief _pose Goal pose to be attached 40 | public: void NewGoal(const ignition::math::Pose3d &_pose); 41 | 42 | /// \brief Update harness state 43 | /// \brief _time Sim time 44 | public: void Update(const common::Time &_time); 45 | 46 | /// \brief Send a message requesting detach. 47 | public: void TriggerDetach(); 48 | 49 | /// \brief Send a message requesting attach, according to the latest pose 50 | /// goal. 51 | public: void TriggerAttach(); 52 | 53 | /// \brief Send a message requesting to lower the harness. 54 | public: void TriggerLower(); 55 | 56 | /// \brief Send a message requesting to switch to high level control. 57 | public: void TriggerStand(); 58 | 59 | /// \brief Returns true if harness is detached 60 | /// \return True if detached 61 | public: bool IsDetached(); 62 | 63 | /// \brief Returns true if harness is attached 64 | /// \return True if attached 65 | public: bool IsAttached(); 66 | 67 | /// \brief Returns true if harness has been lowered enough 68 | /// \return True if lowered 69 | public: bool IsLowered(); 70 | 71 | /// \brief Returns true if robot can stand without the harness 72 | /// \return True if standing 73 | public: bool IsStanding(); 74 | 75 | /// \brief Reset joint positions to neutral configuration. 76 | public: void ResetJointPositions(); 77 | 78 | /// \brief Callback when receiving wrench message from ankle. 79 | /// \param[in] _msg The message 80 | private: void OnSensorMsg(ConstWrenchStampedPtr &_msg); 81 | 82 | /// \brief Enum of possible transitions 83 | private: enum Transition 84 | { 85 | NONE, 86 | DETACH_TO_ATTACH, 87 | ATTACH_TO_LOWER, 88 | LOWER_TO_STAND, 89 | STAND_TO_DETACH, 90 | DETACH_TO_NONE, 91 | }; 92 | 93 | /// \brief Current transition 94 | private: Transition transition = NONE; 95 | 96 | /// \brief latest pose goal 97 | private: ignition::math::Pose3d goal; 98 | 99 | /// \brief True if a new goal has been received 100 | private: bool goalChanged = false; 101 | 102 | /// \brief Pointer to model being harnessed (hardcoded valkyrie) 103 | private: gazebo::physics::ModelPtr model; 104 | 105 | /// \brief Stores force on the ankle from latest sensor message. 106 | private: double latestAnkleForce = 0.0; 107 | 108 | /// \brief Number of iterations to check sensor before being sure that robot 109 | /// has been lowered or is standing. 110 | private: const unsigned int itThreshold = 500; 111 | 112 | /// \brief How many iterations has it been lowered 113 | private: unsigned int itLowering = 0; 114 | 115 | /// \brief How many iterations has it been standing 116 | private: unsigned int itStanding = 0; 117 | 118 | /// \brief Gazebo node for communication 119 | private: gazebo::transport::NodePtr gzNode; 120 | 121 | /// \brief Publishes attach messages 122 | private: gazebo::transport::PublisherPtr attachGzPub; 123 | 124 | /// \brief Publishes detach messages 125 | private: gazebo::transport::PublisherPtr detachGzPub; 126 | 127 | /// \brief Publishes lower messages 128 | private: gazebo::transport::PublisherPtr lowerGzPub; 129 | 130 | /// \brief Subscribe to sensor messages 131 | private: gazebo::transport::SubscriberPtr sensorGzSub; 132 | 133 | /// \brief Ros node handle 134 | private: std::unique_ptr rosNode; 135 | 136 | /// \brief Ros publisher for control messages. 137 | private: ros::Publisher controlRosPub; 138 | 139 | /// \brief Ros publisher for harness status messages. 140 | private: ros::Publisher statusRosPub; 141 | 142 | // Singleton implementation 143 | private: friend class SingletonT; 144 | }; 145 | /// \} 146 | } 147 | #endif 148 | -------------------------------------------------------------------------------- /src/Task1.cc: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2016 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #include "srcsim/HarnessManager.hh" 19 | #include "srcsim/Task1.hh" 20 | 21 | using namespace gazebo; 22 | 23 | ///////////////////////////////////////////////// 24 | Task1::Task1(const sdf::ElementPtr &_sdf) : Task(_sdf) 25 | { 26 | gzmsg << "Creating Task [1] ... "; 27 | 28 | // Get SDF for each checkpoint 29 | sdf::ElementPtr cp1Elem, cp2Elem, cp4Elem; 30 | if (_sdf) 31 | { 32 | if (_sdf->HasElement("checkpoint1")) 33 | cp1Elem = _sdf->GetElement("checkpoint1"); 34 | 35 | if (_sdf->HasElement("checkpoint2")) 36 | cp2Elem = _sdf->GetElement("checkpoint2"); 37 | 38 | if (_sdf->HasElement("checkpoint4")) 39 | cp4Elem = _sdf->GetElement("checkpoint4"); 40 | } 41 | 42 | // Checkpoint 1: Walk to satellite dish 43 | std::unique_ptr cp1(new Task1CP1(cp1Elem)); 44 | this->checkpoints.push_back(std::move(cp1)); 45 | 46 | // Checkpoints 2: Correct pitch or yaw 47 | std::unique_ptr cp2(new Task1CP2(cp2Elem)); 48 | this->checkpoints.push_back(std::move(cp2)); 49 | 50 | // Checkpoints 3: Correct pitch and yaw 51 | // it is not supported to skip the previous checkpoint and start with this 52 | // therefore simply reusing checkpoint 2 53 | std::unique_ptr cp3(new Task1CP3(cp2Elem)); 54 | this->checkpoints.push_back(std::move(cp3)); 55 | 56 | // Checkpoints 3: Walk to final box 57 | std::unique_ptr cp4(new Task1CP4(cp4Elem)); 58 | this->checkpoints.push_back(std::move(cp4)); 59 | 60 | this->logFilter = "satellite_dish*|valkyrie*"; 61 | 62 | gzmsg << "Task [1] created" << std::endl; 63 | } 64 | 65 | ///////////////////////////////////////////////// 66 | size_t Task1::Number() const 67 | { 68 | return 1u; 69 | } 70 | 71 | ///////////////////////////////////////////////// 72 | bool Task1CP1::Check() 73 | { 74 | return this->CheckBox("/task1/checkpoint1"); 75 | } 76 | 77 | ///////////////////////////////////////////////// 78 | void Task1CP1::Restart(const common::Time &_penalty) 79 | { 80 | // This is the 1st CP of the task: reharness back at start box 81 | HarnessManager::Instance()->NewGoal( 82 | ignition::math::Pose3d(0, 0, 1.257, 0, 0, 0)); 83 | 84 | Checkpoint::Restart(_penalty); 85 | } 86 | 87 | ///////////////////////////////////////////////// 88 | void Task1CP2::OnSatelliteRosMsg(const srcsim::Satellite &_msg) 89 | { 90 | this->oneAxisDone = _msg.yaw_completed || _msg.pitch_completed; 91 | } 92 | 93 | ///////////////////////////////////////////////// 94 | bool Task1CP2::Check() 95 | { 96 | // First time 97 | if (!this->satelliteRosSub && !this->oneAxisDone) 98 | { 99 | this->Start(); 100 | 101 | // Subscribe to satellite msgs 102 | this->rosNode.reset(new ros::NodeHandle()); 103 | this->satelliteRosSub = this->rosNode->subscribe( 104 | "/task1/checkpoint2/satellite", 10, &Task1CP2::OnSatelliteRosMsg, this); 105 | 106 | this->gzNode = transport::NodePtr(new transport::Node()); 107 | this->gzNode->Init(); 108 | 109 | // Enable satellite plugin 110 | auto togglePub = this->gzNode->Advertise( 111 | "/task1/checkpoint2/enable"); 112 | 113 | msgs::Int msg; 114 | msg.set_data(1); 115 | togglePub->Publish(msg); 116 | } 117 | 118 | return this->oneAxisDone; 119 | } 120 | 121 | ///////////////////////////////////////////////// 122 | void Task1CP3::OnSatelliteRosMsg(const srcsim::Satellite &_msg) 123 | { 124 | this->satelliteDone = _msg.yaw_completed && _msg.pitch_completed; 125 | } 126 | 127 | ///////////////////////////////////////////////// 128 | bool Task1CP3::Check() 129 | { 130 | // First time 131 | if (!this->satelliteRosSub && !this->satelliteDone) 132 | { 133 | this->Start(); 134 | 135 | // Subscribe to satellite msgs 136 | this->rosNode.reset(new ros::NodeHandle()); 137 | this->satelliteRosSub = this->rosNode->subscribe( 138 | "/task1/checkpoint2/satellite", 10, &Task1CP3::OnSatelliteRosMsg, this); 139 | } 140 | 141 | if (this->satelliteDone && this->rosNode) 142 | { 143 | // Gazebo node 144 | this->gzNode = transport::NodePtr(new transport::Node()); 145 | this->gzNode->Init(); 146 | 147 | // Disable satellite plugin 148 | auto togglePub = this->gzNode->Advertise( 149 | "/task1/checkpoint2/enable"); 150 | 151 | msgs::Int msg; 152 | msg.set_data(0); 153 | togglePub->Publish(msg); 154 | 155 | this->rosNode.reset(); 156 | } 157 | 158 | return this->satelliteDone; 159 | } 160 | 161 | ///////////////////////////////////////////////// 162 | bool Task1CP4::Check() 163 | { 164 | return this->CheckBox("/task1/checkpoint4"); 165 | } 166 | 167 | -------------------------------------------------------------------------------- /include/srcsim/Checkpoint.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef SRC_CHECKPOINT_HH_ 19 | #define SRC_CHECKPOINT_HH_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | namespace gazebo 27 | { 28 | class Checkpoint 29 | { 30 | /// \brief Constructor 31 | /// \param[in] _sdf SDF element for this checkpoint. 32 | public: Checkpoint(const sdf::ElementPtr &_sdf); 33 | 34 | /// \brief Default destructor 35 | public: virtual ~Checkpoint() = default; 36 | 37 | /// \brief Check whether checkpoint has been completed. 38 | /// Any publishers or subscribers are created the first time this is 39 | /// called, and cleaned up once it returns true. 40 | /// \return True if completed. 41 | public: virtual bool Check() = 0; 42 | 43 | /// \brief Skip this checkpoint. 44 | /// This function rearranges the world as if the checkpoint had been 45 | /// completed. 46 | /// The base implementation teleports the robot, but checkpoints can 47 | /// override the function to move other objects too. 48 | public: virtual void Skip(); 49 | 50 | /// \brief Call this the first time the checkpoint is checked. 51 | public: virtual void Start(); 52 | 53 | /// \brief Restart this checkpoint, and increment the total penalty time. 54 | /// \param[in] _penalty Penalty time to add 55 | public: virtual void Restart(const common::Time &_penalty); 56 | 57 | /// \brief Get the total penalty time for this checkpoint. 58 | /// \return The total penalty time. 59 | public: common::Time PenaltyTime() const; 60 | 61 | /// \brief Get the sim time when this checkpoint started. 62 | /// \return Start time 63 | public: common::Time StartTime() const; 64 | 65 | /// \brief The pose the robot should be in when this checkpoint is 66 | /// restarted. This is only used by the first checkpoint of a task, otherwise 67 | /// we start from the skip pose of the previous task. 68 | protected: ignition::math::Pose3d robotStartPose; 69 | 70 | /// \brief The pose the robot should be in when this checkpoint is skipped. 71 | private: ignition::math::Pose3d robotSkipPose; 72 | 73 | /// \brief List of names of entities which should be deleted when this 74 | /// checkpoint starts. 75 | private: std::vector deleteEntities; 76 | 77 | /// \brief List of SDF file strings of entities which should be inserted when 78 | /// this checkpoint starts. 79 | private: std::vector insertEntities; 80 | 81 | /// \brief Total penalty time. 82 | private: common::Time totalPenalty; 83 | 84 | /// \brief Sim time when the checkpoint started 85 | private: common::Time startTime; 86 | }; 87 | 88 | /// \brief A checkpoint tied to a BoxContainsPlugin. 89 | class BoxCheckpoint : public Checkpoint 90 | { 91 | using Checkpoint::Checkpoint; 92 | 93 | /// \brief Check whether the box checkpoint has been completed. 94 | /// \return True if completed. 95 | protected: bool CheckBox(const std::string &_namespace); 96 | 97 | /// \brief Callback when messages are received from the BoxContainsPlugin. 98 | /// \param[in] _msg 1 if robot is inside box, 0 otherwise. 99 | private: void OnBox(ConstIntPtr &_msg); 100 | 101 | /// \brief Gazebo transport node for communication. 102 | protected: transport::NodePtr gzNode; 103 | 104 | /// \brief Gazebo subscriber of box contains messages. 105 | private: transport::SubscriberPtr boxSub; 106 | 107 | /// \brief Gazebo publisher of toggle messages. 108 | private: transport::PublisherPtr togglePub; 109 | 110 | /// \brief Flag to indicate whether the robot has reached the box. 111 | private: bool boxDone = false; 112 | }; 113 | 114 | /// \brief A checkpoint tied to a TouchPlugin. 115 | class TouchCheckpoint : public Checkpoint 116 | { 117 | using Checkpoint::Checkpoint; 118 | 119 | /// \brief Callback when a touch message is received. 120 | /// This means the touch is complete. 121 | /// \param[in] _msg Unused message. 122 | public: void OnTouchGzMsg(ConstIntPtr &_msg); 123 | 124 | /// \brief Check whether the touch checkpoint has been completed. 125 | /// \param[in] _namespace Namespace for the Gazebo topics in this plugin. 126 | /// \return True if completed. 127 | protected: bool CheckTouch(const std::string &_namespace); 128 | 129 | /// \brief Gazebo transport node for communication. 130 | protected: transport::NodePtr gzNode; 131 | 132 | /// \brief Gazebo subscriber of touch messages. 133 | private: transport::SubscriberPtr touchGzSub; 134 | 135 | /// \brief Gazebo publisher of enable messages. 136 | private: transport::PublisherPtr enableGzPub; 137 | 138 | /// \brief Flag to indicate whether the touch is complete. 139 | private: bool touchDone = false; 140 | }; 141 | } 142 | #endif 143 | -------------------------------------------------------------------------------- /models/src_habitat/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | true 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | model://src_habitat/meshes/hdu.obj 15 | 0.0254 0.0254 0.0254 16 | 17 | Stairs 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | model://src_habitat/meshes/hdu.obj 27 | 0.0254 0.0254 0.0254 28 | 29 | Module_Small 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | model://src_habitat/meshes/hdu.obj 39 | 0.0254 0.0254 0.0254 40 | 41 | Door_01 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | model://src_habitat/meshes/hdu.obj 51 | 0.0254 0.0254 0.0254 52 | 53 | Door_03 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | model://src_habitat/meshes/hdu.obj 63 | 0.0254 0.0254 0.0254 64 | 65 | Door_04 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | model://src_habitat/meshes/hdu.obj 75 | 0.0254 0.0254 0.0254 76 | 77 | Module_Large 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | model://src_habitat/meshes/hdu.obj 87 | 0.0254 0.0254 0.0254 88 | 89 | Module_Long 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | model://src_habitat/meshes/hdu.obj 101 | 0.0254 0.0254 0.0254 102 | 103 | Stairs 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | model://src_habitat/meshes/hdu.obj 113 | 0.0254 0.0254 0.0254 114 | 115 | Module_Small 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | model://src_habitat/meshes/hdu.obj 125 | 0.0254 0.0254 0.0254 126 | 127 | Door_01 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | model://src_habitat/meshes/hdu.obj 137 | 0.0254 0.0254 0.0254 138 | 139 | Door_02 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | model://src_habitat/meshes/hdu.obj 149 | 0.0254 0.0254 0.0254 150 | 151 | Door_03 152 | 153 | 154 | 155 | 156 | 157 | 158 | 159 | 160 | model://src_habitat/meshes/hdu.obj 161 | 0.0254 0.0254 0.0254 162 | 163 | Door_04 164 | 165 | 166 | 167 | 168 | 169 | 170 | 171 | 172 | model://src_habitat/meshes/hdu.obj 173 | 0.0254 0.0254 0.0254 174 | 175 | Module_Large 176 | 177 | 178 | 179 | 180 | 181 | 182 | 183 | 184 | model://src_habitat/meshes/hdu.obj 185 | 0.0254 0.0254 0.0254 186 | 187 | Module_Long 188 | 189 | 190 | 191 | 192 | 193 | 194 | 195 | 196 | 197 | -------------------------------------------------------------------------------- /scoring/common.rb: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env ruby 2 | 3 | module Common 4 | # The Vector class represents the generic vector containing elements. 5 | # Since it's commonly used to keep coordinate system 6 | # related information, its elements are labeled by x, y, z. 7 | class Vector 8 | def initialize 9 | @x = 0 10 | @y = 0 11 | @z = 0 12 | end 13 | 14 | def set(x, y, z) 15 | @x = x 16 | @y = y 17 | @z = z 18 | end 19 | 20 | def distance(pt) 21 | Math.sqrt(Math.pow(@x - pt.x, 2) + 22 | Math.pow(@y - pt.y, 2) + 23 | Math.pow(@z - pt.z, 2)) 24 | end 25 | 26 | attr_accessor :x 27 | attr_accessor :y 28 | attr_accessor :z 29 | end 30 | 31 | # A quaternion class. 32 | class Quaternion 33 | def initialize 34 | @x = 0 35 | @y = 0 36 | @z = 0 37 | @w = 1 38 | end 39 | 40 | def set(roll, pitch, yaw) 41 | phi = roll / 2.0 42 | the = pitch / 2.0 43 | psi = yaw / 2.0 44 | 45 | @w = Math.cos(phi) * Math.cos(the) * Math.cos(psi) + 46 | Math.sin(phi) * Math.sin(the) * Math.sin(psi) 47 | @x = Math.sin(phi) * Math.cos(the) * Math.cos(psi) - 48 | Math.cos(phi) * Math.sin(the) * Math.sin(psi) 49 | @y = Math.cos(phi) * Math.sin(the) * Math.cos(psi) + 50 | Math.sin(phi) * Math.cos(the) * Math.sin(psi) 51 | @z = Math.cos(phi) * Math.cos(the) * Math.sin(psi) - 52 | Math.sin(phi) * Math.sin(the) * Math.cos(psi) 53 | normalize 54 | end 55 | 56 | def *(other) 57 | result = Quaternion.new 58 | result.w = @w * other.w - @x * other.x - @y * other.y - @z * other.z 59 | result.x = @w * other.x + @x * other.w + @y * other.z - @z * other.y 60 | result.y = @w * other.y - @x * other.z + @y * other.w + @z * other.x 61 | result.z = @w * other.z + @x * other.y - @y * other.x + @z * other.w 62 | return result 63 | end 64 | 65 | def inverse 66 | q = Quaternion.new 67 | q.w = @w 68 | q.x = @x 69 | q.y = @y 70 | q.z = @z 71 | 72 | s = q.w * q.w + q.x * q.x + q.y * q.y + q.z * q.z 73 | 74 | if s.zero? 75 | q.w = 1.0 76 | q.x = 0.0 77 | q.y = 0.0 78 | q.z = 0.0 79 | else 80 | q.w = q.w / s 81 | q.x = -q.x / s 82 | q.y = -q.y / s 83 | q.z = -q.z / s 84 | end 85 | end 86 | 87 | def normalize 88 | s = Math.sqrt(@w * @w + @x * @x + @y * @y + @z * @z) 89 | 90 | if s.zero? 91 | @w = 1.0 92 | @x = 0.0 93 | @y = 0.0 94 | @z = 0.0 95 | else 96 | @w /= s 97 | @x /= s 98 | @y /= s 99 | @z /= s 100 | end 101 | end 102 | 103 | attr_accessor :x 104 | attr_accessor :y 105 | attr_accessor :z 106 | attr_accessor :w 107 | end 108 | 109 | # \brief Encapsulates a position and rotation in three space. 110 | class Pose 111 | def initialize 112 | @p = Vector.new 113 | @q = Quaternion.new 114 | end 115 | 116 | def set(x, y, z, roll, pitch, yaw) 117 | @p.set(x, y, z) 118 | @q.set(roll, pitch, yaw) 119 | end 120 | 121 | def +(other) 122 | result = Pose.new 123 | 124 | tmp = Quaternion.new 125 | tmp.w = 0.0 126 | tmp.x = other.p.x 127 | tmp.y = other.p.y 128 | tmp.z = other.p.z 129 | tmp = @q * (tmp * @q.inverse) 130 | 131 | result.p.set(@p.x + tmp.x, @p.y + tmp.y, @p.z + tmp.z) 132 | result.q = other.q * @q 133 | 134 | return result 135 | end 136 | 137 | def distance(other) 138 | @p.distance(other.p) 139 | end 140 | 141 | attr_accessor :p 142 | attr_accessor :q 143 | end 144 | 145 | # A Time class, can be used to hold wall- or sim-time 146 | # stored as sec and nano-sec. 147 | class Time 148 | def initialize 149 | @sec = 0 150 | @nsec = 0 151 | end 152 | 153 | def eql?(other) 154 | @sec == other.sec && @nsec == other.nsec 155 | end 156 | 157 | def hash 158 | @sec.to_f + @nsec.to_f * 1e-9 159 | end 160 | 161 | def -(other) 162 | result = Time.new 163 | result.sec = @sec - other.sec 164 | result.nsec = @nsec - other.nsec 165 | result.correct 166 | return result 167 | end 168 | 169 | def >=(other) 170 | if (@sec.to_i < other.sec.to_i) 171 | return false 172 | end 173 | 174 | if (@sec.to_i > other.sec.to_i) 175 | return true 176 | end 177 | 178 | return @nsec.to_i >= other.nsec.to_i 179 | end 180 | 181 | def correct 182 | if @sec > 0 && @nsec < 0 183 | n = (@nsec / 1_000_000_000).abs.to_i + 1 184 | @sec -= n 185 | @nsec += n * 1_000_000_000 186 | end 187 | if @sec < 0 && @nsec > 0 188 | n = (@nsec / 1_000_000_000).abs.to_i + 1 189 | @sec += n 190 | @nsec -= n * 1_000_000_000 191 | end 192 | 193 | @sec += (@nsec / 1_000_000_000).to_i 194 | @nsec = (@nsec % 1_000_000_000).to_i 195 | end 196 | 197 | attr_accessor :sec 198 | attr_accessor :nsec 199 | end 200 | 201 | class Color 202 | def initialize 203 | @r = 0.0 204 | @g = 0.0 205 | @b = 0.0 206 | @a = 1.0 207 | end 208 | 209 | def ==(other) 210 | return @r == other.r && @g == other.g && @b == other.b && @a == other.a 211 | end 212 | 213 | def difference(other) 214 | return Math.sqrt((@r-other.r) * (@r-other.r) + 215 | (@g-other.g) * (@g-other.g) + 216 | (@b-other.b) * (@b-other.b) + 217 | (@a-other.a) * (@a-other.a)) 218 | end 219 | 220 | attr_accessor :r 221 | attr_accessor :g 222 | attr_accessor :b 223 | attr_accessor :a 224 | end 225 | end 226 | -------------------------------------------------------------------------------- /scripts/walk_test.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import time 4 | import rospy 5 | import tf 6 | import tf2_ros 7 | import numpy 8 | 9 | from geometry_msgs.msg import Vector3 10 | from geometry_msgs.msg import Quaternion 11 | 12 | from ihmc_msgs.msg import FootstepStatusRosMessage 13 | from ihmc_msgs.msg import FootstepDataListRosMessage 14 | from ihmc_msgs.msg import FootstepDataRosMessage 15 | 16 | ROBOT_NAME = None 17 | LEFT_FOOT_FRAME_NAME = None 18 | RIGHT_FOOT_FRAME_NAME = None 19 | 20 | def walkTest(): 21 | msg = FootstepDataListRosMessage() 22 | msg.default_transfer_time = 1.5 23 | msg.default_swing_time = 1.5 24 | msg.execution_mode = 0 25 | msg.unique_id = -1 26 | 27 | # walk forward starting LEFT 28 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.LEFT, [0.2, 0.0, 0.0])) 29 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.RIGHT, [0.4, 0.0, 0.0])) 30 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.LEFT, [0.6, 0.0, 0.0])) 31 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.RIGHT, [0.8, 0.0, 0.0])) 32 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.LEFT, [1.0, 0.0, 0.0])) 33 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.RIGHT, [1.2, 0.0, 0.0])) 34 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.LEFT, [1.4, 0.0, 0.0])) 35 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.RIGHT, [1.6, 0.0, 0.0])) 36 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.LEFT, [1.8, 0.0, 0.0])) 37 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.RIGHT, [2.0, 0.0, 0.0])) 38 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.LEFT, [2.2, 0.0, 0.0])) 39 | msg.footstep_data_list.append(createFootStepOffset(FootstepDataRosMessage.RIGHT, [2.4, 0.0, 0.0])) 40 | 41 | footStepListPublisher.publish(msg) 42 | rospy.loginfo('walk forward...') 43 | waitForFootsteps(len(msg.footstep_data_list)) 44 | 45 | # Creates footstep with the current position and orientation of the foot. 46 | def createFootStepInPlace(stepSide): 47 | footstep = FootstepDataRosMessage() 48 | footstep.robot_side = stepSide 49 | 50 | if stepSide == FootstepDataRosMessage.LEFT: 51 | foot_frame = LEFT_FOOT_FRAME_NAME 52 | else: 53 | foot_frame = RIGHT_FOOT_FRAME_NAME 54 | 55 | footWorld = tfBuffer.lookup_transform('world', foot_frame, rospy.Time()) 56 | footstep.orientation = footWorld.transform.rotation 57 | footstep.location = footWorld.transform.translation 58 | 59 | return footstep 60 | 61 | # Creates footstep offset from the current foot position. The offset is in foot frame. 62 | def createFootStepOffset(stepSide, offset): 63 | footstep = createFootStepInPlace(stepSide) 64 | 65 | # transform the offset to world frame 66 | quat = footstep.orientation 67 | rot = tf.transformations.quaternion_matrix([quat.x, quat.y, quat.z, quat.w]) 68 | transformedOffset = numpy.dot(rot[0:3, 0:3], offset) 69 | 70 | footstep.location.x += transformedOffset[0] 71 | footstep.location.y += transformedOffset[1] 72 | footstep.location.z += transformedOffset[2] 73 | 74 | return footstep 75 | 76 | def waitForFootsteps(numberOfSteps): 77 | global stepCounter 78 | stepCounter = 0 79 | while stepCounter < numberOfSteps: 80 | rate.sleep() 81 | rospy.loginfo('finished set of steps') 82 | 83 | def recievedFootStepStatus(msg): 84 | global stepCounter 85 | if msg.status == 1: 86 | stepCounter += 1 87 | 88 | if __name__ == '__main__': 89 | try: 90 | rospy.init_node('ihmc_walk_test') 91 | 92 | if not rospy.has_param('/ihmc_ros/robot_name'): 93 | rospy.logerr("Cannot run walk_test.py, missing parameters!") 94 | rospy.logerr("Missing parameter '/ihmc_ros/robot_name'") 95 | 96 | else: 97 | ROBOT_NAME = rospy.get_param('/ihmc_ros/robot_name') 98 | 99 | right_foot_frame_parameter_name = "/ihmc_ros/{0}/right_foot_frame_name".format(ROBOT_NAME) 100 | left_foot_frame_parameter_name = "/ihmc_ros/{0}/left_foot_frame_name".format(ROBOT_NAME) 101 | 102 | if rospy.has_param(right_foot_frame_parameter_name) and rospy.has_param(left_foot_frame_parameter_name): 103 | RIGHT_FOOT_FRAME_NAME = rospy.get_param(right_foot_frame_parameter_name) 104 | LEFT_FOOT_FRAME_NAME = rospy.get_param(left_foot_frame_parameter_name) 105 | 106 | footStepStatusSubscriber = rospy.Subscriber("/ihmc_ros/{0}/output/footstep_status".format(ROBOT_NAME), FootstepStatusRosMessage, recievedFootStepStatus) 107 | footStepListPublisher = rospy.Publisher("/ihmc_ros/{0}/control/footstep_list".format(ROBOT_NAME), FootstepDataListRosMessage, queue_size=1) 108 | 109 | tfBuffer = tf2_ros.Buffer() 110 | tfListener = tf2_ros.TransformListener(tfBuffer) 111 | 112 | rate = rospy.Rate(10) # 10hz 113 | time.sleep(1) 114 | 115 | # make sure the simulation is running otherwise wait 116 | if footStepListPublisher.get_num_connections() == 0: 117 | rospy.loginfo('waiting for subsciber...') 118 | while footStepListPublisher.get_num_connections() == 0: 119 | rate.sleep() 120 | 121 | if not rospy.is_shutdown(): 122 | walkTest() 123 | else: 124 | if not rospy.has_param(left_foot_frame_parameter_name): 125 | rospy.logerr("Missing parameter {0}".format(left_foot_frame_parameter_name)) 126 | if not rospy.has_param(right_foot_frame_parameter_name): 127 | rospy.logerr("Missing parameter {0}".format(right_foot_frame_parameter_name)) 128 | 129 | except rospy.ROSInterruptException: 130 | pass 131 | -------------------------------------------------------------------------------- /include/srcsim/Task2.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef SRC_TASK2_HH_ 19 | #define SRC_TASK2_HH_ 20 | 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | #include "Checkpoint.hh" 28 | #include "Task.hh" 29 | 30 | namespace gazebo 31 | { 32 | class Task2 : public Task 33 | { 34 | /// \brief Constructor 35 | /// \param[in] _sdf Pointer to SDF element for this task. 36 | public: Task2(const sdf::ElementPtr &_sdf); 37 | 38 | // Documentation inherited 39 | public: size_t Number() const; 40 | }; 41 | 42 | /// \brief Task 2, Checkpoint 1: Lift panel 43 | class Task2CP1 : public TouchCheckpoint 44 | { 45 | using TouchCheckpoint::TouchCheckpoint; 46 | 47 | /// \brief Check whether the panel has been touching the robot and 48 | /// nothing else for long enough. 49 | /// \return True if the checkpoint is complete. 50 | public: bool Check(); 51 | 52 | /// \brief Reharness at start box 53 | /// \param[in] _penalty Penalty time to add 54 | public: void Restart(const common::Time &_penalty); 55 | }; 56 | 57 | /// \brief Task 2, Checkpoint 2: Place panel near cable 58 | class Task2CP2 : public BoxCheckpoint 59 | { 60 | using BoxCheckpoint::BoxCheckpoint; 61 | 62 | /// \brief Check whether the panel is within reach of the cable. 63 | /// \return True if the checkpoint is complete. 64 | public: bool Check(); 65 | 66 | /// \brief Place panel back on the rover. 67 | /// \param[in] _penalty Penalty time to add 68 | public: void Restart(const common::Time &_penalty); 69 | 70 | /// \brief Skip this checkpoint. This places the solar panel on the array. 71 | public: void Skip(); 72 | }; 73 | 74 | /// \brief Task 2, Checkpoint 3: Deploy solar panel 75 | class Task2CP3 : public Checkpoint 76 | { 77 | using Checkpoint::Checkpoint; 78 | 79 | /// \brief Check whether the solar panel has been deployed. 80 | /// \return True if the checkpoint is complete. 81 | public: bool Check(); 82 | 83 | /// \brief Place panel back on the array. It should be still locked. 84 | /// \param[in] _penalty Penalty time to add 85 | public: void Restart(const common::Time &_penalty); 86 | 87 | /// \brief Skip this checkpoint. This deploys the solar panel. 88 | public: void Skip(); 89 | 90 | /// \brief Callback when a message about the solar panel is received. 91 | /// This means the panel has been opened. 92 | /// \param[in] _msg Unused message. 93 | private: void OnSolarPanelGzMsg(ConstIntPtr &/*_msg*/); 94 | 95 | /// \brief Whether the checkpoint is complete or not. 96 | private: bool panelDone = false; 97 | 98 | /// \brief Gazebo transport node for communication. 99 | private: transport::NodePtr gzNode; 100 | 101 | /// \brief Subscribes to solar panel messages. 102 | private: transport::SubscriberPtr panelGzSub; 103 | 104 | /// \brief Publishes enable messages. 105 | private: transport::PublisherPtr enableGzPub; 106 | }; 107 | 108 | /// \brief Task 2, Checkpoint 4: Lift cable 109 | class Task2CP4 : public TouchCheckpoint 110 | { 111 | using TouchCheckpoint::TouchCheckpoint; 112 | 113 | /// \brief Check whether the cable's tip has been touching the robot and 114 | /// nothing else for long enough. 115 | /// \return True if the checkpoint is complete. 116 | public: bool Check(); 117 | 118 | /// \brief Place panel back on the array, deployed. Reset cable pose. 119 | /// \param[in] _penalty Penalty time to add 120 | public: void Restart(const common::Time &_penalty); 121 | }; 122 | 123 | /// \brief Task 2, Checkpoint 5: Plug cable 124 | class Task2CP5 : public Checkpoint 125 | { 126 | using Checkpoint::Checkpoint; 127 | 128 | /// \brief Check whether the robot is in the final box region. 129 | /// \return True if the checkpoint is complete. 130 | public: bool Check(); 131 | 132 | /// \brief Place panel back on the array, deployed. Reset cable pose. 133 | /// \param[in] _penalty Penalty time to add 134 | public: void Restart(const common::Time &_penalty); 135 | 136 | /// \brief Whether the checkpoint is complete or not. 137 | private: bool done = false; 138 | 139 | /// \brief Pointer to contact sensor 140 | private: sensors::ContactSensorPtr sensor; 141 | 142 | /// \brief Pointer to the world 143 | private: physics::WorldPtr world; 144 | 145 | /// \brief Name of contact sensor 146 | private: std::string sensorName = "outlet_sensor"; 147 | 148 | /// \brief Name of cable model 149 | private: std::string cable = "solar_panel_cable"; 150 | 151 | /// \brief Name of link which has the cable plug 152 | private: std::string plug = "solar_panel_cable::link_16"; 153 | 154 | /// \brief Name of link which has the outlet 155 | private: std::string outletParent = "solar_panel::panel_02"; 156 | 157 | /// \brief Name of outlet collision 158 | private: std::string outlet = "solar_panel::panel_02::outlet"; 159 | 160 | /// \brief Time when started touching. 161 | private: common::Time touchStart; 162 | 163 | /// \brief Target time to continuously touch. 164 | private: common::Time targetTime = 0.5; 165 | }; 166 | 167 | /// \brief Task 2, Checkpoint 6: Final box 168 | class Task2CP6 : public BoxCheckpoint 169 | { 170 | using BoxCheckpoint::BoxCheckpoint; 171 | 172 | /// \brief Check whether the robot is in the final box region. 173 | /// \return True if the checkpoint is complete. 174 | public: bool Check(); 175 | }; 176 | } 177 | #endif 178 | -------------------------------------------------------------------------------- /include/srcsim/Task.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (C) 2017 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | 18 | #ifndef SRC_TASK_HH_ 19 | #define SRC_TASK_HH_ 20 | 21 | #include 22 | #include 23 | 24 | #include 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include "Checkpoint.hh" 36 | 37 | namespace gazebo 38 | { 39 | /// \brief Base class for all 3 tasks. 40 | class Task 41 | { 42 | /// \brief Constructor 43 | /// \param[in] _sdf SDF element for this task. 44 | public: Task(const sdf::ElementPtr &_sdf); 45 | 46 | /// \brief Start this task at a specific checkpoint 47 | /// \param[in] _time Start time 48 | /// \param[in] _checkpoint Checkpoint Id 49 | public: void Start(const common::Time &_time, const size_t _checkpoint); 50 | 51 | /// \brief Update this task 52 | /// \param[in] _time Current time 53 | public: void Update(const common::Time &_time); 54 | 55 | /// \brief Skip this task 56 | /// \param[in] _penalty True if skip penalty should be applied. 57 | public: void Skip(const bool _penalty); 58 | 59 | /// \brief Return the number of checkpoints in this task. 60 | /// \return Number of checkpoints. 61 | public: size_t CheckpointCount() const; 62 | 63 | /// \brief Return the index of the current checkpoint. 64 | /// \return Index of current checkpoint. 65 | public: size_t CurrentCheckpointId() const; 66 | 67 | /// \brief Return the total time taken to complete a checkpoint. 68 | /// \param[in] _index Index of the checkpoint in the array (0~n-1) 69 | /// \return The competion time, zero if it wasn't complete. 70 | public: common::Time GetCheckpointCompletion(const size_t index) const; 71 | 72 | /// \brief Return the total time penalty taken by a checkpoint. 73 | /// \param[in] _index Index of the checkpoint in the array (0~n-1) 74 | /// \return The sum of all penalties for a checkpoint. 75 | public: common::Time GetCheckpointPenalty(const size_t index) const; 76 | 77 | /// \brief Return this task's number. 78 | /// \return Task number. 79 | public: virtual size_t Number() const = 0; 80 | 81 | /// \brief Skip all remaining checkpoints up to the given one, inclusive. 82 | /// \param[in] _lastSkipped Id of last checkpoint to be skipped (1..n). 83 | /// \param[in] _penalty True if skip penalty should be applied. 84 | public: void SkipUpTo(const size_t _lastSkipped, const bool _penalty); 85 | 86 | /// \brief Callback when messages are received from the BoxContainsPlugin. 87 | /// \param[in] _msg 1 if robot is inside box, 0 otherwise. 88 | private: void OnStartBox(ConstIntPtr &_msg); 89 | 90 | /// \brief Increment penalty time for current checkpoint, based on current 91 | /// `previousPenalty`. 92 | private: void ApplyPenaltyTime(); 93 | 94 | /// \brief Vector of checkpoints for this task. 95 | /// checkpoints[0]: Checkpoint 1 96 | /// checkpoints[1]: Checkpoint 2 97 | /// checkpoints[2]: Checkpoint 3 98 | /// ... 99 | protected: std::vector > checkpoints; 100 | 101 | /// \brief Current checkpoint number, starting from 1. Zero means the task 102 | /// hasn't started, Count+1 means that the task has finished. 103 | private: size_t current = 0; 104 | 105 | /// \brief Gazebo transport node for communication. 106 | protected: transport::NodePtr gzNode; 107 | 108 | /// \brief Filter to only log a few models during this task 109 | protected: std::string logFilter; 110 | 111 | /// \brief Vector of total times each checkpoint took to complete. That 112 | /// doesn't include time penalties. 113 | /// Time is zero for skipped checkpoints. 114 | private: std::vector cpDuration; 115 | 116 | /// \brief Time when the task started 117 | private: common::Time startTime; 118 | 119 | /// \brief Total time allowed for the task. 120 | private: common::Time timeout = 300; 121 | 122 | /// \brief True if task has been completed before timeout. 123 | private: bool finished = false; 124 | 125 | /// \brief True if task has timed out before completion. 126 | private: bool timedOut = false; 127 | 128 | /// \brief Total time penalty due to resets and skips 129 | private: common::Time totalPenalty; 130 | 131 | /// \brief Previous time penalty due to resets and skips. This carries 132 | /// over from one task to the next. 133 | private: static common::Time previousPenalty; 134 | 135 | /// \brief Ros node handle 136 | private: std::unique_ptr rosNode; 137 | 138 | /// \brief Ros publisher which publishes the task's status. 139 | private: ros::Publisher taskRosPub; 140 | 141 | /// \brief Gazebo subscriber of box contains messages. 142 | private: transport::SubscriberPtr boxSub; 143 | 144 | /// \brief Gazebo publisher of toggle messages. 145 | private: transport::PublisherPtr togglePub; 146 | 147 | /// \brief Flag to indicate whether the robot has reached the start box. 148 | private: bool startBox = false; 149 | 150 | /// \brief Mutex used to protect the update loop 151 | private: std::mutex updateMutex; 152 | 153 | /// \brief Ros force checkpoint completion subscriber 154 | private: ros::Subscriber forceCpCompletionRosSub; 155 | 156 | /// \brief Callback when a force checkpoint completion message is received. 157 | /// \param[in] _msg Force checkpoint completion message 158 | private: void OnForceCpCompletionRosMsg( 159 | const std_msgs::Empty::ConstPtr &_msg); 160 | 161 | /// \brief Flag to indicate forcing checkpoint completion. 162 | private: bool forceCpCompletion = false; 163 | }; 164 | } 165 | #endif 166 | -------------------------------------------------------------------------------- /include/srcsim/SRCMultiSenseSLPlugin.hh: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright 2012-2016 Open Source Robotics Foundation 3 | * 4 | * Licensed under the Apache License, Version 2.0 (the "License"); 5 | * you may not use this file except in compliance with the License. 6 | * You may obtain a copy of the License at 7 | * 8 | * http://www.apache.org/licenses/LICENSE-2.0 9 | * 10 | * Unless required by applicable law or agreed to in writing, software 11 | * distributed under the License is distributed on an "AS IS" BASIS, 12 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 13 | * See the License for the specific language governing permissions and 14 | * limitations under the License. 15 | * 16 | */ 17 | #ifndef SRCMULTISENSE_SL_PLUGIN_HH_ 18 | #define SRCMULTISENSE_SL_PLUGIN_HH_ 19 | 20 | #include 21 | 22 | #include 23 | #include 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | 55 | namespace gazebo 56 | { 57 | class SRCMultiSenseSL : public ModelPlugin 58 | { 59 | /// \brief Constructor 60 | /// \param parent The parent entity, must be a Model 61 | public: SRCMultiSenseSL(); 62 | 63 | /// \brief Destructor 64 | public: ~SRCMultiSenseSL(); 65 | 66 | /// \brief Load the plugin 67 | /// \param[in] _parent pointer to parent Model 68 | /// \param[in] _sdf SDF root element corresponds to the plugin XML block 69 | public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf); 70 | 71 | /// \brief Update the controller periodically via Events. 72 | protected: virtual void UpdateStates(); 73 | 74 | /// \brief Pointer to the update event connection 75 | private: event::ConnectionPtr updateConnection; 76 | 77 | /// \brief Thread for loading and initializing ROS 78 | private: void LoadThread(); 79 | private: boost::thread deferred_load_thread_; 80 | 81 | // IMU sensor 82 | private: sensors::ImuSensorPtr imuSensor; 83 | private: std::string imuLinkName; 84 | private: physics::LinkPtr imuLink; 85 | private: ros::Publisher pubImu; 86 | private: PubQueue::Ptr pubImuQueue; 87 | 88 | // reset of ros stuff 89 | private: ros::NodeHandle* rosnode_; 90 | private: ros::CallbackQueue queue_; 91 | private: void QueueThread(); 92 | private: boost::thread callback_queue_thread_; 93 | 94 | // ros topic subscriber 95 | private: ros::Subscriber set_spindle_speed_sub_; 96 | private: void SetSpindleSpeed(const std_msgs::Float64::ConstPtr &_msg); 97 | 98 | private: ros::Subscriber set_spindle_state_sub_; 99 | private: void SetSpindleState(const std_msgs::Bool::ConstPtr &_msg); 100 | 101 | /// for deprecation from ~/multisense_sl/fps to ~/multisense_sl/set_fps 102 | /// per issue 272 103 | private: ros::Subscriber set_multi_camera_frame_rate_sub_old_; 104 | private: void SetMultiCameraFrameRateOld(const std_msgs::Float64::ConstPtr 105 | &_msg); 106 | 107 | private: ros::Subscriber set_multi_camera_frame_rate_sub_; 108 | private: void SetMultiCameraFrameRate(const std_msgs::Float64::ConstPtr 109 | &_msg); 110 | 111 | private: ros::Subscriber set_multi_camera_resolution_sub_; 112 | 113 | private: std::string rosNamespace; 114 | 115 | /// \brief camera resolution modes 116 | /// available modes are: 117 | /// 0 - 2MP (2048*1088) @ up to 15 fps 118 | /// 1 - 1MP (1536*816) @ up to 30 fps 119 | /// 2 - 0.5MP (1024*544) @ up to 60 fps (default) 120 | /// 3 - VGA (640*480) @ up to 70 fps 121 | private: void SetMultiCameraResolution( 122 | const std_msgs::Int32::ConstPtr &_msg); 123 | 124 | private: ros::Subscriber set_multi_camera_exposure_time_sub_; 125 | private: void SetMultiCameraExposureTime(const std_msgs::Float64::ConstPtr 126 | &_msg); 127 | 128 | private: ros::Subscriber set_multi_camera_gain_sub_; 129 | private: void SetMultiCameraGain(const std_msgs::Float64::ConstPtr 130 | &_msg); 131 | 132 | // ros services 133 | private: ros::ServiceServer set_spindle_state_service_; 134 | private: bool SetSpindleState(std_srvs::Empty::Request &req, 135 | std_srvs::Empty::Response &res); 136 | 137 | private: ros::ServiceServer set_spindle_speed_service_; 138 | private: bool SetSpindleSpeed(std_srvs::Empty::Request &req, 139 | std_srvs::Empty::Response &res); 140 | 141 | // gazebo variables 142 | private: physics::WorldPtr world; 143 | private: physics::ModelPtr valModel; 144 | private: sdf::ElementPtr sdf; 145 | private: common::Time lastTime; 146 | 147 | // joint state 148 | private: ros::Publisher pubJointStates; 149 | private: PubQueue::Ptr pubJointStatesQueue; 150 | private: sensor_msgs::JointState jointStates; 151 | 152 | // camera control 153 | private: sensors::MultiCameraSensorPtr multiCameraSensor; 154 | private: double multiCameraFrameRate; 155 | private: double multiCameraExposureTime; 156 | private: double multiCameraGain; 157 | private: int imagerMode; 158 | 159 | // spindle control 160 | private: double spindleSpeed; 161 | private: double spindleMaxRPM; 162 | private: double spindleMinRPM; 163 | private: bool spindleOn; 164 | private: physics::LinkPtr spindleLink; 165 | private: physics::JointPtr spindleJoint; 166 | private: common::PID spindlePID; 167 | 168 | /// Throttle update rate 169 | private: double lastUpdateTime; 170 | private: double updateRate; 171 | 172 | // ros publish multi queue, prevents publish() blocking 173 | private: PubMultiQueue* pmq; 174 | }; 175 | } 176 | #endif 177 | --------------------------------------------------------------------------------