Full Code Framework

- Add node for controlling arm
- Add node for cv model
- Add trained cv model data
- Add node for handling depth camera
- Add node for spawning in new items
- Add main node to handle interactions with other nodes

rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp

@@ -0,0 +1,83 @@
+// arm_controller_node.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+
+#include <ros/ros.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+
+Class ArmController {
+public:
+    ArmController(ros::NodeHandle & node) : arm_1_has_been_zeroed {
+        arm_1_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>("/ariac/arm1/arm/command", 10);
+    }
+
+    void arm_1_joint_state_callback(const sensor_msgs::JointState::ConstPtr & joint_state_msg) {
+        ROS_INFO_STREAM_THROTTLE(10, "Joint States arm 1 (throttled to 0.1 Hz):\n" << *joint_state_msg);
+        // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg);
+        arm_1_current_joint_states_ = *joint_state_msg;
+        if (!arm_1_has_been_zeroed_) {
+            arm_1_has_been_zeroed_ = true;
+            ROS_INFO("Sending arm to zero joint positions...");
+            send_arm_to_zero_state(arm_1_joint_trajectory_publisher_);
+        }
+    }
+
+    /// Create a JointTrajectory with all positions set to zero, and command the arm.
+    void send_arm_to_zero_state(ros::Publisher & joint_trajectory_publisher) {
+        // Create a message to send.
+        trajectory_msgs::JointTrajectory msg;
+
+        // Fill the names of the joints to be controlled.
+        // Note that the vacuum_gripper_joint is not controllable.
+        msg.joint_names.clear();
+        msg.joint_names.push_back("shoulder_pan_joint");
+        msg.joint_names.push_back("shoulder_lift_joint");
+        msg.joint_names.push_back("elbow_joint");
+        msg.joint_names.push_back("wrist_1_joint");
+        msg.joint_names.push_back("wrist_2_joint");
+        msg.joint_names.push_back("wrist_3_joint");
+        msg.joint_names.push_back("linear_arm_actuator_joint");
+        // Create one point in the trajectory.
+        msg.points.resize(1);
+        // Resize the vector to the same length as the joint names.
+        // Values are initialized to 0.
+        msg.points[0].positions.resize(msg.joint_names.size(), 0.0);
+        // How long to take getting to the point (floating point seconds).
+        msg.points[0].time_from_start = ros::Duration(0.001);
+        ROS_INFO_STREAM("Sending command:\n" << msg);
+        joint_trajectory_publisher.publish(msg);
+    }
+
+private:
+    ros::Publisher arm_1_joint_trajectory_publisher_;
+    sensor_msgs::JointState arm_1_current_joint_states_;
+    bool arm_1_has_been_zeroed_;
+}
+
+
+// TODO: Determine message type
+void location_callback(const sensor_msgs::/*MessageType*/::ConstPtr & msg) {
+    
+}
+
+
+int main(int argc, char ** argv) {
+    // Last argument is the default name of the node.
+    ros::init(argc, argv, "arm_controller_node");
+
+    ros::NodeHandle node;
+    
+    // TODO: Subscribe to topic to receive current and destination locations
+    ros::Subscriber sub = node.subscribe(/*topic*/, 1, location_callback);
+
+    // TODO: Move arm to pickup item from conveyor belt and turn on suction
+
+    // TODO: Move item to desired position and turn off suction
+
+}

rrrobot_ws/src/rrrobot/src/cv_model.py

@@ -0,0 +1,77 @@
+#!/usr/bin/env python
+import rospy
+from std_msgs.msg import String
+
+import os, torch, torchvision
+import numpy as np
+from torch import nn
+from PIL import Image
+
+
+class Model_v1(nn.Module):
+    def __init__(self):
+        super(Model_v1, self).__init__()
+        self.resnet = torchvision.models.resnet34(pretrained=True)
+        self.resnet.fc = torch.nn.Linear(512, 512)
+        self.addition = nn.Sequential(
+            nn.BatchNorm1d(512),
+            nn.Dropout(p=0.5),
+            nn.Linear(512, 6))
+
+    def forward(self, image):
+        z = self.resnet(image)
+        z = self.addition(z)
+        return z
+
+
+def call_back(filename):
+    # prepare input file
+    input_image = Image.open(filename)
+    input_image = input_image.resize((512, 384))
+    # print(input_image.size)
+    input_image = np.moveaxis(np.asarray(input_image), 2, 0).astype(np.float32)
+    input_tensor = torch.from_numpy(input_image)
+    input_tensor = torch.unsqueeze(input_tensor, dim=0)
+    # print(input_tensor.shape)
+
+    # query model
+    predicted_logits = model(input_tensor).squeeze()
+    # print(predicted_logits.shape)
+    predicted_label = torch.argmax(predicted_logits).numpy().tolist()
+
+    type_dict = {0: 'cardboard',
+                 1: 'glass'    ,
+                 2: 'metal'    ,
+                 3: 'paper'    ,
+                 4: 'plastic'  ,
+                 5: 'trash'    }
+
+    print('type: ', type_dict[predicted_label])
+
+    # publish a message, name of this node is 'cv_model'
+    ​pub = rospy.Publisher('cv_model', String, queue_size=10)
+​    ​rospy.init_node('talker', anonymous=True)
+​    ​rate = rospy.Rate(10) # 10hz
+    
+    ​garbage_type = type_dict[predicted_label]
+    ​rospy.loginfo(filename + ':' + garbage_type)
+    ​pub.publish(filename + ':' + garbage_type)
+    ​rate.sleep()
+
+
+def listener():
+    rospy.init_node('listener', anonymous=True)
+    rospy.Subscriber('/ariac/logical_camera_1', String, callback)
+    # spin() simply keeps python from exiting until this node is stopped
+    rospy.spin()
+
+
+if __name__ == '__main__':
+    # prepare pretrained model
+    model = Model_v1()
+    model.load_state_dict(torch.load('v1_0.001.pt', map_location='cpu'))
+    model = model.to('cpu')
+    model.eval()
+
+    # listen to another node
+    listener()

rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp

@@ -0,0 +1,57 @@
+// depth_camera_node.cpp
+
+#include <algorithm>
+#include <vector>
+#include <string>
+
+#include <ros/ros.h>
+#include "sensor_msgs/PointCloud.h"
+#include "sensor_msgs/PointCloud2.h"
+#include "message_filters/subscriber.h"
+#include "pcl/io/pcd_io.h"
+#include "pcl/point_types.h"
+#include <pcl_conversions/pcl_conversions.h>
+#include <pcl/point_cloud.h>
+#include <pcl/point_types.h>
+#include <pcl/segmentation/extract_clusters.h>
+
+#include <std_msgs/Float32.h>
+#include <std_msgs/String.h>
+#include <std_srvs/Trigger.h>
+
+
+pcl::PointCloud<pcl::PointXYZ> convert_pc(const sensor_msgs::PointCloud::ConstPtr & msg) {
+    // Convert from sensor_msgs/PointCloud.msg to necessary format
+    // Convert to PointCloud2
+    sensor_msgs::PointCloud2 cloud2 = new sensor_msgs::PointCloud2;
+    sensor_msgs::PointCloudToPointCloud2(msg, cloud2); 
+
+    // Convert to PCL data type
+    pcl::PointCloud<pcl::PointXYZ> output = new pcl::PointCloud<pcl::PointXYZ>;
+    pcl::fromROSMsg(cloud2, output);
+}
+
+void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & msg) {
+    pcl::PointCloud<pcl::PointXYZ> cloud = convert_pc(msg);
+
+    // extract clusters
+
+}
+
+int main(int argc, char ** argv) {
+    // TODO: Subscribe to depth camera topic
+    // Last argument is the default name of the node.
+    ros::init(argc, argv, "depth_camera_node");
+
+    ros::NodeHandle node;
+    
+    ros::Subscriber sub = node.subscribe("/ariac/depth_camera_1", 1, depth_camera_callback);
+
+    
+    
+    // TODO: When item is in view, work with point cloud to get location (in world frame) for arm to reach to pickup item
+    
+
+    // TODO: Publish object's current location to topic for RRRobot node to listen to
+    
+}

rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp

@@ -101,6 +101,120 @@ private:
     }
 };
 
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "object_spawner");
+
+    ObjectSpawner spawner;
+
+    ros::spin();
+}           // y range: anything (essentially)
+            // z
+            msg.pose.position.x = DEFAULT_SPAWN_POINT.x() /* + some random error */;
+            msg.pose.position.y = DEFAULT_SPAWN_POINT.y();
+            msg.pose.position.z = DEFAULT_SPAWN_POINT.z() + 1.0;
+
+            pub.publish(msg);
+            ros::spinOnce();
+        }
+    }
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "object_spawner");
+
+    ObjectSpawner spawner;
+
+    ros::spin();
+}           // y range: anything (essentially)
+            // z
+            msg.pose.position.x = DEFAULT_SPAWN_POINT.x() /* + some random error */;
+            msg.pose.position.y = DEFAULT_SPAWN_POINT.y();
+            msg.pose.position.z = DEFAULT_SPAWN_POINT.z() + 1.0;
+
+            pub.publish(msg);
+            ros::spinOnce();
+        }
+    }
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "object_spawner");
+
+    ObjectSpawner spawner;
+
+    ros::spin();
+}           // y range: anything (essentially)
+            // z
+            msg.pose.position.x = DEFAULT_SPAWN_POINT.x() /* + some random error */;
+            msg.pose.position.y = DEFAULT_SPAWN_POINT.y();
+            msg.pose.position.z = DEFAULT_SPAWN_POINT.z() + 1.0;
+
+            pub.publish(msg);
+            ros::spinOnce();
+        }
+    }
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "object_spawner");
+
+    ObjectSpawner spawner;
+
+    ros::spin();
+}           // y range: anything (essentially)
+            // z
+            msg.pose.position.x = DEFAULT_SPAWN_POINT.x() /* + some random error */;
+            msg.pose.position.y = DEFAULT_SPAWN_POINT.y();
+            msg.pose.position.z = DEFAULT_SPAWN_POINT.z() + 1.0;
+
+            pub.publish(msg);
+            ros::spinOnce();
+        }
+    }
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "object_spawner");
+
+    ObjectSpawner spawner;
+
+    ros::spin();
+}           // y range: anything (essentially)
+            // z
+            msg.pose.position.x = DEFAULT_SPAWN_POINT.x() /* + some random error */;
+            msg.pose.position.y = DEFAULT_SPAWN_POINT.y();
+            msg.pose.position.z = DEFAULT_SPAWN_POINT.z() + 1.0;
+
+            pub.publish(msg);
+            ros::spinOnce();
+        }
+    }
+};
+
+int main(int argc, char **argv)
+{
+    ros::init(argc, argv, "object_spawner");
+
+    ObjectSpawner spawner;
+
+    ros::spin();
+}           // y range: anything (essentially)
+            // z
+            msg.pose.position.x = DEFAULT_SPAWN_POINT.x() /* + some random error */;
+            msg.pose.position.y = DEFAULT_SPAWN_POINT.y();
+            msg.pose.position.z = DEFAULT_SPAWN_POINT.z() + 1.0;
+
+            pub.publish(msg);
+            ros::spinOnce();
+        }
+    }
+};
+
 int main(int argc, char **argv)
 {
     ros::init(argc, argv, "object_spawner");

rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp

@@ -12,10 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-// %Tag(FULLTEXT)%
-// %Tag(INCLUDE_STATEMENTS)%
 #include <algorithm>
 #include <vector>
+#include <string>
+#include <stdlib.h>
+#include <time.h>
+#include <fstream>
 
 #include <ros/ros.h>
 
@@ -29,31 +31,6 @@
 #include <std_msgs/String.h>
 #include <std_srvs/Trigger.h>
 #include <trajectory_msgs/JointTrajectory.h>
-// %EndTag(INCLUDE_STATEMENTS)%
-
-// %Tag(START_COMP)%
-/// Start the competition by waiting for and then calling the start ROS Service.
-void start_competition(ros::NodeHandle & node) {
-  // Create a Service client for the correct service, i.e. '/ariac/start_competition'.
-  ros::ServiceClient start_client =
-    node.serviceClient<std_srvs::Trigger>("/ariac/start_competition");
-  // If it's not already ready, wait for it to be ready.
-  // Calling the Service using the client before the server is ready would fail.
-  if (!start_client.exists()) {
-    ROS_INFO("Waiting for the competition to be ready...");
-    start_client.waitForExistence();
-    ROS_INFO("Competition is now ready.");
-  }
-  ROS_INFO("Requesting competition start...");
-  std_srvs::Trigger srv;  // Combination of the "request" and the "response".
-  start_client.call(srv);  // Call the start Service.
-  if (!srv.response.success) {  // If not successful, print out why.
-    ROS_ERROR_STREAM("Failed to start the competition: " << srv.response.message);
-  } else {
-    ROS_INFO("Competition started!");
-  }
-}
-// %EndTag(START_COMP)%
 
 /// Example class that can hold state and provide methods that handle incoming data.
 class MyCompetitionClass
@@ -62,40 +39,10 @@ public:
   explicit MyCompetitionClass(ros::NodeHandle & node)
   : current_score_(0), arm_1_has_been_zeroed_(false), arm_2_has_been_zeroed_(false)
   {
-    // %Tag(ADV_CMD)%
     arm_1_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
       "/ariac/arm1/arm/command", 10);
-
-    arm_2_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
-      "/ariac/arm2/arm/command", 10);
-    // %EndTag(ADV_CMD)%
   }
 
-  /// Called when a new message is received.
-  void current_score_callback(const std_msgs::Float32::ConstPtr & msg) {
-    if (msg->data != current_score_)
-    {
-      ROS_INFO_STREAM("Score: " << msg->data);
-    }
-    current_score_ = msg->data;
-  }
-
-  /// Called when a new message is received.
-  void competition_state_callback(const std_msgs::String::ConstPtr & msg) {
-    if (msg->data == "done" && competition_state_ != "done")
-    {
-      ROS_INFO("Competition ended.");
-    }
-    competition_state_ = msg->data;
-  }
-
-  /// Called when a new Order message is received.
-  void order_callback(const osrf_gear::Order::ConstPtr & order_msg) {
-    ROS_INFO_STREAM("Received order:\n" << *order_msg);
-    received_orders_.push_back(*order_msg);
-  }
-
-  // %Tag(CB_CLASS)%
   /// Called when a new JointState message is received.
   void arm_1_joint_state_callback(
     const sensor_msgs::JointState::ConstPtr & joint_state_msg)
@@ -111,22 +58,6 @@ public:
     }
   }
 
-  void arm_2_joint_state_callback(
-    const sensor_msgs::JointState::ConstPtr & joint_state_msg)
-  {
-    ROS_INFO_STREAM_THROTTLE(10,
-      "Joint States arm 2 (throttled to 0.1 Hz):\n" << *joint_state_msg);
-    // ROS_INFO_STREAM("Joint States:\n" << *joint_state_msg);
-    arm_2_current_joint_states_ = *joint_state_msg;
-    if (!arm_2_has_been_zeroed_) {
-      arm_2_has_been_zeroed_ = true;
-      ROS_INFO("Sending arm 2 to zero joint positions...");
-      send_arm_to_zero_state(arm_2_joint_trajectory_publisher_);
-    }
-  }
-  // %EndTag(CB_CLASS)%
-
-  // %Tag(ARM_ZERO)%
   /// Create a JointTrajectory with all positions set to zero, and command the arm.
   void send_arm_to_zero_state(ros::Publisher & joint_trajectory_publisher) {
     // Create a message to send.
@@ -152,50 +83,85 @@ public:
     ROS_INFO_STREAM("Sending command:\n" << msg);
     joint_trajectory_publisher.publish(msg);
   }
-  // %EndTag(ARM_ZERO)%
-
-  /// Called when a new LogicalCameraImage message is received.
-  void logical_camera_callback(
-    const osrf_gear::LogicalCameraImage::ConstPtr & image_msg)
-  {
-    ROS_INFO_STREAM_THROTTLE(10,
-      "Logical camera: '" << image_msg->models.size() << "' objects.");
-  }
-
-  /// Called when a new Proximity message is received.
-  void break_beam_callback(const osrf_gear::Proximity::ConstPtr & msg) {
-    if (msg->object_detected) {  // If there is an object in proximity.
-      ROS_INFO("Break beam triggered.");
-    }
-  }
 
 private:
-  std::string competition_state_;
-  double current_score_;
   ros::Publisher arm_1_joint_trajectory_publisher_;
-  ros::Publisher arm_2_joint_trajectory_publisher_;
-  std::vector<osrf_gear::Order> received_orders_;
   sensor_msgs::JointState arm_1_current_joint_states_;
-  sensor_msgs::JointState arm_2_current_joint_states_;
   bool arm_1_has_been_zeroed_;
-  bool arm_2_has_been_zeroed_;
 };
 
-void proximity_sensor_callback(const sensor_msgs::Range::ConstPtr & msg) {
-  if ((msg->max_range - msg->range) > 0.01) {  // If there is an object in proximity.
-    ROS_INFO_THROTTLE(1, "Proximity sensor sees something.");
+class Position {
+  public:
+    float x, y, z;
+
+    Position(float x, float y, float z) : x(x), y(y), z(z) {};
+};
+
+Position destination(String type) {
+	Position pos;
+	float z = 1;
+  
+	if (type == "cardboard") {
+    	pos = Position(-0.3, -1.916, z);
   	}
+  	else if (type == "glass") {
+    	pos = Position(-0.3, -1.15, z);
+  	}
+  	else if (type == "metal") {
+    	pos = Position(-0.3, -0.383, z);
+  	}
+  	else if (type == "paper") {
+    	pos = Position(-0.3, 0.383, z);
+  	}
+  	else if (type == "plastic")
+  	{
+    	pos = Position(-0.3, 1.15, z);
+  	}
+  	else if (type == "trash")
+  	{
+    	pos = Position(-0.3, 1.916, z);
+  	}
+
+  	return pos;
 }
 
-void laser_profiler_callback(const sensor_msgs::LaserScan::ConstPtr & msg) {
-  size_t number_of_valid_ranges = std::count_if(
-    msg->ranges.begin(), msg->ranges.end(), [](const float f) {return std::isfinite(f);});
-  if (number_of_valid_ranges > 0) {
-    ROS_INFO_THROTTLE(1, "Laser profiler sees something.");
+void conveyor_controller(ros::NodeHandle node, int power) {
+	if (power != 0 && (power < 50 || power > 100)) {
+		return;
 	}
+
+	ros::ServiceClient client = node.serviceClient<osrf_gear::ConveyorBeltControl>("/ariac/conveyor/control");
+	osrf_gear::ConveyorBeltControl cmd;
+	cmd.power = power;
+	client.call(cmd);
+
+	return;
 }
 
-// %Tag(MAIN)%
+
+// void spawn_items(ros::NodeHandle & node) {
+// 	ros::Publisher pub = node.advertise<rrrobot::model_insertion>("/object_to_load", 1000);
+
+// 	if (ros::ok())
+// 	{
+// 		rrrobot::model_insertion msg;
+		
+// 		// TODO: Randomly select model to spawn
+// 		// msg.model_name = "SOMETHING";
+
+// 		// TODO: Determine position to spawn items
+// 		// can place objects on conveyor belt just upstream from the arms 
+// 		// at roughly (0.238416, 5.474367, 0.935669)
+// 		msg.pose.position.x = 0;
+// 		msg.pose.position.y = 0;
+// 		msg.pose.position.z = 0;
+
+// 		pub.publish(msg);
+// 		ros::spinOnce();
+// 	}
+// }
+
+
 int main(int argc, char ** argv) {
 	// Last argument is the default name of the node.
 	ros::init(argc, argv, "rrrobot_node");
@@ -205,51 +171,29 @@ int main(int argc, char ** argv) {
 	// Instance of custom class from above.
 	MyCompetitionClass comp_class(node);
 
-  // Subscribe to the '/ariac/current_score' topic.
-  ros::Subscriber current_score_subscriber = node.subscribe(
-    "/ariac/current_score", 10,
-    &MyCompetitionClass::current_score_callback, &comp_class);
-
-  // Subscribe to the '/ariac/competition_state' topic.
-  ros::Subscriber competition_state_subscriber = node.subscribe(
-    "/ariac/competition_state", 10,
-    &MyCompetitionClass::competition_state_callback, &comp_class);
-
-  // %Tag(SUB_CLASS)%
-  // Subscribe to the '/ariac/orders' topic.
-  ros::Subscriber orders_subscriber = node.subscribe(
-    "/ariac/orders", 10,
-    &MyCompetitionClass::order_callback, &comp_class);
-  // %EndTag(SUB_CLASS)%
-
 	// Subscribe to the '/ariac/joint_states' topic.
 	ros::Subscriber arm_1_joint_state_subscriber = node.subscribe(
 		"/ariac/arm1/joint_states", 10,
 		&MyCompetitionClass::arm_1_joint_state_callback, &comp_class);
 	
-  ros::Subscriber arm_2_joint_state_subscriber = node.subscribe(
-    "/ariac/arm2/joint_states", 10,
-    &MyCompetitionClass::arm_2_joint_state_callback, &comp_class);
+	// TODO: Spawn items (random order)
+	//    - Make a call to gazebo model insert script
+	spawn_items(node);
 
-  // %Tag(SUB_FUNC)%
-  // Subscribe to the '/ariac/proximity_sensor_1' topic.
-  ros::Subscriber proximity_sensor_subscriber = node.subscribe(
-    "/ariac/proximity_sensor_1", 10, proximity_sensor_callback);
-  // %EndTag(SUB_FUNC)%
+	// TODO: Listen to cv_model topic for classification
+	// ros::Subscriber laser_profiler_subscriber = node.subscribe(
+	//   "/ariac/laser_profiler_1", 10, laser_profiler_callback);
 
-  // Subscribe to the '/ariac/break_beam_1_change' topic.
-  ros::Subscriber break_beam_subscriber = node.subscribe(
-    "/ariac/break_beam_1_change", 10,
-    &MyCompetitionClass::break_beam_callback, &comp_class);
+	// TODO: Listen to depth camera handler
+	//    - Pass store current object location
+	//    - Make call to arm controller (see below)
 
-  // Subscribe to the '/ariac/logical_camera_1' topic.
-  ros::Subscriber logical_camera_subscriber = node.subscribe(
-    "/ariac/logical_camera_1", 10,
-    &MyCompetitionClass::logical_camera_callback, &comp_class);
-
-  // Subscribe to the '/ariac/laser_profiler_1' topic.
-  ros::Subscriber laser_profiler_subscriber = node.subscribe(
-    "/ariac/laser_profiler_1", 10, laser_profiler_callback);
+	// TODO: Start arm controller
+	//    - Stop conveyor belt
+	//    - Send destination and current location to arm controller
+	//    - Wait to receive successful drop-off signal
+	//    - Start conveyor belt
+	//    - Spawn new items
 
 	ROS_INFO("Setup complete.");
 	start_competition(node);
@@ -257,5 +201,3 @@ int main(int argc, char ** argv) {
 
 	return 0;
 }
-// %EndTag(MAIN)%
-// %EndTag(FULLTEXT)%