Merge pull request #9 from cwru-robotics/master

updaate

README.md

@@ -1,61 +1,12 @@
 # cwru_ariac_2018
 for NIST ARIAC competition
 
-example:
-Start up demo simulation:
-`roslaunch osrf_gear sample_environment.launch`
-or,  to test with faulty part:
-`roslaunch osrf_gear sample_environment.launch fill_demo_shipment:=true`
+Start up one of the simulation scenarios (choose 1 from below):
 
-better:
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/quals/qual1a.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual1_config.yaml`
-
-
-rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/quals/qual1a.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual1_config.yaml 
-
-or, for qual2 setup:
-
-rosrun osrf_gear gear.py --visualize-sensor-views -f `catkin_find --share --first-only osrf_gear`/config/sample.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml
-
-ON ATLAS6 USE:
-rosrun osrf_gear gear.py --visualize-sensor-views -f `catkin_find --share --first-only osrf_gear`/config/sample.yaml ~/ros_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml
-
-Qualifier 2:
-4 piston rods in bin1, 2 flanges in bin2, 2 disks in bin3, bin4 is empty, 3 gears in bin5
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/quals/qual2a.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
-
-The following ariac config files present different scenarios:
-sample_dropped_products.yaml, sample_flipped.yaml, sample_interruption1.yaml, sample_interruption2.yaml, 
-sample_not_enough_products.yaml, sample_order_update.yaml, sample_sensor_blackout.yaml, sample.yaml
-
-Run these variations, e.g., on Ava (wsn laptop) with:
-
-The following setup is trouble--gears close together, and piston rods in bin5 (not ready for bin5)
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample_dropped_products.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
-
-The following has pulley parts--not ready to do pulley flips.  Also has piston-rod parts in bin5
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample_interruption1.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
-
-The following has no pulleys and no parts in bin5, but gear parts are very close together:
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample_interruption2.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
-
-The following has piston-rod parts in bin5; the rest are disk parts in bins 1 and 2
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample_not_enough_products.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
-
-order-update: order change during filling!   has tightly-spaced gears, pulleys, and bin5 piston rods
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample_order_update.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
-
-sample blackout: has piston rods in bin2, gears in bin3 and flanges in bin4
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample_sensor_blackout.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
-
-sample: 
-`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample.yaml ~/ariac_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
-
-
-
-ON ATLAS6:
 `rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/quals/qual2a.yaml ~/ros_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
 
+`rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/quals/qual2b.yaml ~/ros_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
+
 `rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample_dropped_products.yaml ~/ros_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
 
 `rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample_interruption1.yaml ~/ros_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
@@ -70,22 +21,21 @@ ON ATLAS6:
 
 `rosrun osrf_gear gear.py -f `catkin_find --share --first-only osrf_gear`/config/sample.yaml ~/ros_ws/ariac-docker/team_config/team_case_config/qual2_config.yaml`
 
+Start up 4 of our nodes:
 
 Start up robot move action server:
-`rosrun kuka_move_as kuka_behavior_as`
+`rosrun kuka_move_as kuka_behavior_as2`
+
+Start up the conveyor action server:
+`rosrun conveyor_as conveyor_as`
 
-start the shipment filler:
-`rosrun shipment_filler simple_shipment_filler`
+Start up the shipment selection service (this will get changed to a new name/version):
+`rosrun optimizer_func dumb_optimizer`
 
-For isolated tests, e.g.
-For a simple test that grabs items from inventory, puts them in a box, then discards them, run:
-`rosrun shipment_filler test_part_placement_from_inventory`
+Start up the  new shipment filler (top-level node):
+`rosrun shipment_filler simple_shipment_filler_w_optimizer`
 
-Send a test order:
-`rosrun order_sender order_sender2`
 
-OR, send orders via competition interface:
-`rosservice call /ariac/start_competition`
 
 
 

box_inspector/include/box_inspector/box_inspector.h

@@ -99,6 +99,9 @@ class BoxInspector
   bool model_poses_wrt_box(osrf_gear::Shipment &shipment_status);
   bool compare_pose(geometry_msgs::Pose , geometry_msgs::Pose);
   bool compare_pose(geometry_msgs::PoseStamped, geometry_msgs::PoseStamped);
+  bool compare_pose_approx(geometry_msgs::Pose pose_A, geometry_msgs::Pose pose_B);
+  bool compare_pose_approx(geometry_msgs::PoseStamped, geometry_msgs::PoseStamped);
+
   bool post_dropoff_check(vector<osrf_gear::Model> desired_models_wrt_world,vector<osrf_gear::Model> &misplaced_models_desired_coords, vector<osrf_gear::Model> &misplaced_models_actual_coords);
   bool pre_dropoff_check(inventory_msgs::Part part, osrf_gear::Model model_actual, osrf_gear::Model model_desired);
   geometry_msgs::PoseStamped NOM_BOX1_POSE_WRT_WORLD,NOM_BOX2_POSE_WRT_WORLD;