Code Cleanup & Commenting

- Added file name as comment to top of file
- Add some brief comments explaining what functions do
- Removed code that was commented out
- Add whitespace
- Change using std::<function> to using namespace std

src/rrrobot_ws/src/rrrobot/include/arm_representation.h

@@ -1,4 +1,4 @@
-// arm_controller_node.cpp
+// arm_representation.h
 
 #include <algorithm>
 #include <vector>

src/rrrobot_ws/src/rrrobot/include/topic_names.h

@@ -1,3 +1,5 @@
+// topic_names.h
+
 // COMPUTER VISION
 #define CV_CLASSIFICATION_CHANNEL   "/cv_model"                     // CV Model classifications
 

src/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp

@@ -45,6 +45,7 @@ using namespace std;
 class ArmController
 {
 public:
+    // Constructor
     ArmController(ros::NodeHandle &node, ArmRepresentation *arm_, double time_per_step, int retry_attempts, double item_attach_z_adjustment) : gripper_enabled_(false), item_attached_(false), arm(arm_), time_per_step(time_per_step), retry_attempts(retry_attempts), item_attach_z_adjustment(item_attach_z_adjustment)
     {
         arm_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(ARM_COMMAND_CHANNEL, 10);
@@ -55,28 +56,24 @@ public:
 
         /* initialize random seed: */
         srand(time(NULL));
-
-        // arm = std::move(arm_);
-        // ROS_INFO("Arm size (in constructor): %d", arm->getChain()->getNrOfJoints());
-        // arm_current_joint_states_ = KDL::JntArray(arm->getChain()->getNrOfJoints());
-        // KDL::SetToZero(arm_current_joint_states_);
     }
 
+    // Receive joint state messages
     void joint_state_callback(const sensor_msgs::JointState &joint_state_msg)
     {
-        // ROS_INFO("Received joint state");
-
-        // Convert std_msgs::double[] to KDL::JntArray
+        // Copy joint states to private variable arm_current_joint_states_
         int nbr_joints = arm->getChain()->getNrOfJoints();
         vector<string> msg_joint_names = joint_state_msg.name;
         vector<string> cur_joint_names = arm->get_joint_names();
         vector<double> position = joint_state_msg.position;
 
+        // Set all values to 0
         for (int i = 0; i < nbr_joints; ++i)
         {
             arm_current_joint_states_[i] = 0.0;
         }
 
+        // Match up joint names from message to internal joint names order
         for (size_t i = 0; i < position.size(); ++i)
         {
             for (size_t j = 0; j < cur_joint_names.size(); ++j)
@@ -89,12 +86,15 @@ public:
         }
     }
 
+    // Receive gripper state messages
     void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr &gripper_state_msg)
     {
+        // Store message states in private variables
         gripper_enabled_ = gripper_state_msg->enabled;
         item_attached_ = gripper_state_msg->attached;
     }
 
+    // Receive message from RRRobot node with desired pose to pick up object from conveyor belt
     void arm_destination_callback(const rrrobot::arm_command &target_pose)
     {
         ROS_INFO("Received target pose");
@@ -161,9 +161,8 @@ public:
 
         // Move item to desired position
         attempts = 1;
-        // if (calc_joint_positions(target_pose.drop_location, above_bins, positions))
-        //     send_joint_trajectory(positions, arm_action_phase::belt_to_bin);
         KDL::Frame end_effector_pose;
+        
         if (target_pose.drop_location.position.y > 1.0)
         {
             positions = bin1;
@@ -194,7 +193,6 @@ public:
                 return;
             }
 
-            // if (calc_joint_positions(target_pose.grab_location, get_randomized_start_state(above_bins) /*target_pose.drop_location.position.y)*/, positions))
             send_joint_trajectory(positions, arm_action_phase::belt_to_bin);
             attempts++;
         }
@@ -256,26 +254,24 @@ private:
     const vector<double> bin1 = {0.05, 0.0, -2.2, -2.2, -0.25, 1.5708, 0.0};
     const vector<double> bin2 = {-0.7, 0.0, -2.2, -2.2, -0.25, 1.5708, 0.0};
 
+    // Randomize start state to avoid inverse kinematics getting stuck in local minimum
     vector<double> get_randomized_start_state(const vector<double> &preferred_state)
     {
-        //int state_to_return = rand() % 2;
-
-        // if (state == 0)
-        //     return preferred_state;
-
         vector<double> cur;
         std::stringstream location;
         location << "Random location: ";
         // cur.push_back(y);
-        for (size_t pos = 0; pos < arm->getChain()->getNrOfJoints(); ++pos)
-        {
+        for (size_t pos = 0; pos < arm->getChain()->getNrOfJoints(); ++pos) {
             int val = rand() % 2;
             double diff = 0;
-            if (val == 1)
+            
+            if (val == 1) {
                 diff = -0.1;
-            else
+            }
+            else {
                 diff = 0.1;
-            // double val = (rand() % 70) / 100.0;
+            }
+            
             cur.push_back(preferred_state[pos] + diff);
             location << cur[pos] << " ";
         }
@@ -286,10 +282,9 @@ private:
         return cur;
     }
 
+    // Use inverse kinematics to calculate joint positions to reach desired pose 
     bool calc_joint_positions(const geometry_msgs::Pose &pose, const vector<double> &start_state, vector<double> &positions)
     {
-        // KDL::JntArray cur_configuration(arm->getChain()->getNrOfJoints()); // = arm_current_joint_states_;
-        // KDL::SetToZero(cur_configuration);
         KDL::Frame desired_end_effector_pose = pose_to_frame(pose);
         KDL::JntArray final_joint_configuration(arm->getChain()->getNrOfJoints());
 
@@ -306,16 +301,16 @@ private:
         // Convert data attribute (Eigen::VectorXd) of KDL::JntArray to double[] via data() function
         int nbr_joints = arm->getChain()->getNrOfJoints();
         Eigen::VectorXd mat = final_joint_configuration.data;
-        // cout << mat << endl;
         positions.clear();
-        // vector<double> positions;
         std::stringstream pos_str;
         pos_str << "Calculated joint positions: ";
+        
         for (size_t idx = 0; idx < arm->getChain()->getNrOfJoints(); ++idx)
         {
             positions.push_back(mat[idx]);
             pos_str << mat[idx] << " ";
         }
+        
         pos_str << '\n';
 
         ROS_INFO(pos_str.str().c_str());
@@ -323,6 +318,7 @@ private:
         return (error_code == 0);
     }
 
+    // Use forward kinematics to calculate end effector pose from current joint states
     KDL::Frame calc_end_effector_pose()
     {
         int num_joints = arm->getChain()->getNrOfJoints();
@@ -347,6 +343,7 @@ private:
         return end_effector_pose;
     }
 
+    // Convert from Pose to KDL::Frame
     KDL::Frame pose_to_frame(const geometry_msgs::Pose &pose)
     {
         double p_x = pose.position.x;
@@ -363,6 +360,7 @@ private:
         return KDL::Frame(rot, pos);
     }
 
+    // Convert from KDL::Frame to Pose
     geometry_msgs::Pose frame_to_pose(const KDL::Frame &frame)
     {
         double p_x = frame.p.x();
@@ -386,6 +384,7 @@ private:
         return pose;
     }
 
+    // Send desired joint states to joint controller 
     void send_joint_trajectory(const vector<double> &target, arm_action_phase phase)
     {
         // Declare JointTrajectory message
@@ -448,6 +447,7 @@ private:
         ros::Duration((num_points + 1) * time_per_step).sleep();
     }
 
+    // Turn gripper on or off
     bool gripper_control(bool state)
     {
         osrf_gear::VacuumGripperControl srv;
@@ -463,9 +463,10 @@ private:
         return success;
     }
 
+    // Check if the end effector has reached the target position (within threshold)
     bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
     {
-        // Tune threshold values
+        // Threshold values
         float pos_thresh = 0.15; // Meters
         float rot_thresh = 0.05;
 
@@ -485,10 +486,10 @@ private:
         float qz_err = fabs(fabs(cur.orientation.z) - fabs(target.orientation.z));
         float qw_err = fabs(fabs(cur.orientation.w) - fabs(target.orientation.w));
 
-        ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")");
-        ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")");
-        ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")");
-        ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")");
+        // ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")");
+        // ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")");
+        // ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")");
+        // ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")");
 
         if (qx_err > rot_thresh)
         {
@@ -516,7 +517,7 @@ private:
 
 int main(int argc, char **argv)
 {
-    cout << "Starting arm_controller_node" << endl;
+    ROS_INFO("Starting arm_controller_node");
 
     // Last argument is the default name of the node.
     ros::init(argc, argv, "arm_controller_node");
@@ -525,19 +526,19 @@ int main(int argc, char **argv)
 
     ArmRepresentation arm;
 
+    // Set parameters
     double time_per_step = 3.0; // seconds
     int retry_attempts = 3;
     double item_attach_z_adjustment = 0.05; // meters
     ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment);
 
+    // Start asynchronous spinner to receive messages on subscribed topics
     ros::AsyncSpinner spinner(0);
     spinner.start();
 
-    // Subscribe to arm destination and joint states channels
+    // Subscribe to ROS topics
     ros::Subscriber arm_destination_sub = node.subscribe(ARM_DESTINATION_CHANNEL, 1, &ArmController::arm_destination_callback, &ac);
-
     ros::Subscriber gripper_state_sub = node.subscribe(GRIPPER_STATE_CHANNEL, 1, &ArmController::gripper_state_callback, &ac);
-
     ros::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 1, &ArmController::joint_state_callback, &ac);
 
     ROS_INFO("Setup complete");

src/rrrobot_ws/src/rrrobot/src/arm_representation.cpp

@@ -1,14 +1,14 @@
-// arm_controller_node.cpp
+// arm_representation.cpp
 
 #include "arm_representation.h"
 #include <math.h>
 
-// using namespace std;
-using std::string;
-using std::vector;
+using namespace std;
 
+// Constructor
 ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
 {
+    // Parameters for coordinate transformations of robot links and joints
     const double base_len = 0.2;
     const double shoulder_height = 0.1273;
     const double upper_arm_length = 0.612;
@@ -19,10 +19,7 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
     const double wrist_2_length = 0.1157;
     const double wrist_3_length = 0.0922;
 
-    // KDL::Vector pos; //(0, 0, base_len/2);
-    // KDL::Rotation rot;
-
-    // The joints might be off by one
+    // Define chain of links and joints using KDL
     KDL::Segment linear_arm_actuator("linear_arm_actuator_joint",
                                      KDL::Joint(KDL::Joint::JointType::None), base_pose);
     chain.addSegment(linear_arm_actuator);
@@ -74,23 +71,9 @@ ArmRepresentation::ArmRepresentation(const KDL::Frame &base_pose)
     KDL::Segment ee_link("ee_link", KDL::Joint("wrist_3_joint", KDL::Joint::JointType::RotY), //KDL::Joint(KDL::Joint::JointType::None),
                          KDL::Frame(rot_ee, pos_ee));
     chain.addSegment(ee_link);
-
-    // arm.addSegment(base_link);
-    // arm.addSegment(shoulder_link);
-    // arm.addSegment(upper_arm_link);
-    // arm.addSegment(forearm_link);
-    // arm.addSegment(wrist_1_link);
-    // arm.addSegment(wrist_2_link);
-    // arm.addSegment(wrist_3_link);
-    // arm.addSegment(ee_link);
-
-    // fk_solver = KDL::ChainFkSolverPos_recursive(chain);
-    // ik_solver = KDL::ChainIkSolverPos_LMA(chain);
-
-    // fk_solver.reset(new KDL::ChainFkSolverPos_recursive(arm));
-    // ik_solver.reset(new KDL::ChainIkSolverPos_LMA(arm));
 }
 
+// Use KDL to calculate position of end effector from joint states
 int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_positions, KDL::Frame &end_effector_pose, int joint_nbr)
 {
     KDL::ChainFkSolverPos_recursive fk_solver(chain);
@@ -99,6 +82,7 @@ int ArmRepresentation::calculateForwardKinematics(const KDL::JntArray &joint_pos
     return status;
 }
 
+// Use KDL to calculate joint states from position of end effector
 int ArmRepresentation::calculateInverseKinematics(const vector<double> &cur_configuration,
                                                   const KDL::Frame &desired_end_effector_pose,
                                                   KDL::JntArray &final_joint_configuration)
@@ -119,6 +103,7 @@ int ArmRepresentation::calculateInverseKinematics(const vector<double> &cur_conf
     return status;
 }
 
+// Get vector of joint names
 vector<string> ArmRepresentation::get_joint_names()
 {
     vector<string> joint_names;
@@ -132,6 +117,7 @@ vector<string> ArmRepresentation::get_joint_names()
     return joint_names;
 }
 
+// Return reference to object
 KDL::Chain *ArmRepresentation::getChain()
 {
     return &chain;

src/rrrobot_ws/src/rrrobot/src/cv_model.py

@@ -1,4 +1,6 @@
 #!/usr/bin/env python
+# cv_model.py
+
 import rospy
 from std_msgs.msg import String
 

src/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp

@@ -252,21 +252,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
 
 		// publish pose
 		pub.publish(pose);
-
-		// compute normals
 	}
-
-	//std::cerr<< "segmented:  " << (int)point_cloud_segmented->size() << "\n";
-
-	// Convert to ROS data type
-	//point_cloud_segmented->header.frame_id = point_cloudPtr->header.frame_id;
-	//if(point_cloud_segmented->size()) pcl::toPCLPointCloud2(*point_cloud_segmented, cloud_filtered);
-	//else pcl::toPCLPointCloud2(*point_cloudPtr, cloud_filtered);
-	//sensor_msgs::PointCloud2 output;
-	//pcl_conversions::fromPCL(cloud_filtered, output);
-
-	// Publish the data
-	//pub.publish (output);
 }
 
 int main(int argc, char **argv)

src/rrrobot_ws/src/rrrobot/src/image_display.py

@@ -1,4 +1,6 @@
 #!/usr/bin/env python
+# image_display.py
+
 import rospy
 from std_msgs.msg import String
 import matplotlib.pyplot as plt

src/rrrobot_ws/src/rrrobot/src/model_insertion_plugin.cpp

@@ -1,3 +1,5 @@
+// model_insertion_plugin.cpp
+
 #include <ignition/math/Pose3.hh>
 #include "gazebo/physics/physics.hh"
 #include "gazebo/common/common.hh"

src/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp

@@ -1,3 +1,5 @@
+// object_spawner_node.cpp
+
 #include <vector>
 #include <string>
 #include <stdlib.h>
@@ -13,8 +15,7 @@
 #include <kdl/frames.hpp>
 
 #include <iostream>
-using std::cout;
-using std::endl;
+using namespace std;
 
 class ObjectSpawner
 {

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

@@ -1,16 +1,4 @@
-// Copyright 2016 Open Source Robotics Foundation, Inc.
-//
-// Licensed under the Apache License, Version 2.0 (the "License");
-// you may not use this file except in compliance with the License.
-// You may obtain a copy of the License at
-//
-//     http://www.apache.org/licenses/LICENSE-2.0
-//
-// Unless required by applicable law or agreed to in writing, software
-// distributed under the License is distributed on an "AS IS" BASIS,
-// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
-// See the License for the specific language governing permissions and
-// limitations under the License.
+// rrrobot_node.cpp
 
 #include <algorithm>
 #include <vector>
@@ -38,8 +26,7 @@
 #include "topic_names.h"
 #include "rrrobot/arm_command.h"
 
-using std::cout;
-using std::endl;
+using namespace std;
 
 class Position
 {
@@ -78,7 +65,6 @@ public:
 		desired_drop_pose.position.x = drop_point.x;
 		desired_drop_pose.position.y = drop_point.y;
 		desired_drop_pose.position.z = drop_point.z;
-		// TODO: set the desired orientation of the end effector to point straight down
 		
 		// update state
 		if (current_robot_state & RobotState::WAITING_FOR_GRAB_LOCATION)
@@ -93,7 +79,7 @@ public:
 			publish_arm_command();
 		}
 
-		print_state();
+		// print_state();
 	}
 
 	void gripper_state_callback(const osrf_gear::VacuumGripperState &state)
@@ -117,7 +103,7 @@ public:
 		// store current state
 		gripper_state = state;
 
-		print_state();
+		// print_state();
 	}
 
 	void grasp_pose_callback(const geometry_msgs::Pose &grasp_pose)
@@ -126,7 +112,7 @@ public:
 		set_conveyor(0);
 
 		desired_grasp_pose = grasp_pose;
-		// TODO: Tune z offset so end effector doesn't hit object
+		// Add z offset so end effector doesn't hit object
 		desired_grasp_pose.position.z += 0.01;
 
 		if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
@@ -140,7 +126,7 @@ public:
 			publish_arm_command();
 		}
 
-		print_state();
+		// print_state();
 	}
 
 private:
@@ -168,6 +154,7 @@ private:
 	Position trash_bin = Position(-0.3, 0.383, 1);
 	Position recycle_bin = Position(-0.3, 1.15, 1);
 
+	// Determine item destination bin based on classification
 	Position destination(const std::string &type) const
 	{
 		Position pos;
@@ -184,6 +171,7 @@ private:
 		return pos;
 	}
 
+	// Publish message including grab and drop off location for arm controller
 	void publish_arm_command()
 	{
 		rrrobot::arm_command cmd;
@@ -194,6 +182,7 @@ private:
 		ros::spinOnce();
 	}
 
+	// Set conveyor power (0 or 50-100)
 	void set_conveyor(int power)
 	{
 		if (power != 0 && (power < 50 || power > 100))
@@ -206,6 +195,7 @@ private:
 		conveyor_pub.call(cmd);
 	}
 
+	// Print current state for debugging
 	void print_state()
 	{
 		if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)

src/rrrobot_ws/src/rrrobot/test/test_arm.cpp

@@ -1,4 +1,4 @@
-// arm_controller_node.cpp
+// test_arm.cpp
 
 #include <algorithm>
 #include <vector>

src/rrrobot_ws/src/rrrobot/test/test_insert_object.cpp

@@ -1,3 +1,5 @@
+// test_insert_object.cpp
+
 #include "rrrobot/model_insertion.h"
 #include "ros/ros.h"