Merge branch 'GEAR_arm_controller' of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot into GEAR_arm_controller

src/rrrobot_ws/src/gazebo_models/pop_can/model.sdf

@@ -12,9 +12,9 @@
           <iyz>0</iyz>
           <izz>0.166667</izz>
         </inertia>
-        <pose frame=''>0 0 0 0 -0 0</pose>
+        <pose frame=''>0.02 -0.05 0.023 0 -0 0</pose>
       </inertial>
-      <pose frame=''>0 0 0 1.5708 -0 0</pose>
+      <pose frame=''>-0 0 0.08 1.5708 -0 0</pose>
       <gravity>1</gravity>
       <self_collide>0</self_collide>
       <kinematic>0</kinematic>

src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp

@@ -18,6 +18,7 @@
 #include <stdlib.h>
 #include <time.h>
 #include <fstream>
+#include <iostream>
 
 #include <ros/ros.h>
 
@@ -37,6 +38,9 @@
 #include "topic_names.h"
 #include "rrrobot/arm_command.h"
 
+using std::cout;
+using std::endl;
+
 class Position
 {
 public:
@@ -66,7 +70,7 @@ public:
 		comp_start.call(trg);
 	}
 
-	void cv_classification_callback(const std_msgs::String & classification)
+	void cv_classification_callback(const std_msgs::String &classification)
 	{
 		std::string type = classification.data.substr(classification.data.find(":") + 1);
 
@@ -88,6 +92,8 @@ public:
 			// tell the arm to move to grab the object
 			publish_arm_command();
 		}
+
+		print_state();
 	}
 
 	void gripper_state_callback(const osrf_gear::VacuumGripperState &state)
@@ -110,6 +116,8 @@ public:
 
 		// store current state
 		gripper_state = state;
+
+		print_state();
 	}
 
 	void grasp_pose_callback(const geometry_msgs::Pose &grasp_pose)
@@ -131,6 +139,8 @@ public:
 
 			publish_arm_command();
 		}
+
+		print_state();
 	}
 
 private:
@@ -193,6 +203,23 @@ private:
 		cmd.request.power = power;
 		conveyor_pub.call(cmd);
 	}
+
+	void print_state()
+	{
+		if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
+		{
+			cout << "Waiting for classification\t";
+		}
+		if (current_robot_state & RobotState::WAITING_FOR_GRAB_LOCATION)
+		{
+			cout << "Waiting for grab location\t";
+		}
+		if (current_robot_state & RobotState::MOVING_ARM)
+		{
+			cout << "Moving Arm\t";
+		}
+		cout << endl;
+	}
 };
 
 int main(int argc, char **argv)