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
This commit is contained in:
Sravan Balaji
2020-04-27 13:00:50 -04:00
parent 1490e427bd
commit 686c6081a8
12 changed files with 79 additions and 105 deletions

View File

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

View File

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

View File

@@ -45,6 +45,7 @@ using namespace std;
class ArmController class ArmController
{ {
public: 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) 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); arm_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(ARM_COMMAND_CHANNEL, 10);
@@ -55,28 +56,24 @@ public:
/* initialize random seed: */ /* initialize random seed: */
srand(time(NULL)); 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) void joint_state_callback(const sensor_msgs::JointState &joint_state_msg)
{ {
// ROS_INFO("Received joint state"); // Copy joint states to private variable arm_current_joint_states_
// Convert std_msgs::double[] to KDL::JntArray
int nbr_joints = arm->getChain()->getNrOfJoints(); int nbr_joints = arm->getChain()->getNrOfJoints();
vector<string> msg_joint_names = joint_state_msg.name; vector<string> msg_joint_names = joint_state_msg.name;
vector<string> cur_joint_names = arm->get_joint_names(); vector<string> cur_joint_names = arm->get_joint_names();
vector<double> position = joint_state_msg.position; vector<double> position = joint_state_msg.position;
// Set all values to 0
for (int i = 0; i < nbr_joints; ++i) for (int i = 0; i < nbr_joints; ++i)
{ {
arm_current_joint_states_[i] = 0.0; 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 i = 0; i < position.size(); ++i)
{ {
for (size_t j = 0; j < cur_joint_names.size(); ++j) 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) void gripper_state_callback(const osrf_gear::VacuumGripperState::ConstPtr &gripper_state_msg)
{ {
// Store message states in private variables
gripper_enabled_ = gripper_state_msg->enabled; gripper_enabled_ = gripper_state_msg->enabled;
item_attached_ = gripper_state_msg->attached; 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) void arm_destination_callback(const rrrobot::arm_command &target_pose)
{ {
ROS_INFO("Received target pose"); ROS_INFO("Received target pose");
@@ -161,9 +161,8 @@ public:
// Move item to desired position // Move item to desired position
attempts = 1; 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; KDL::Frame end_effector_pose;
if (target_pose.drop_location.position.y > 1.0) if (target_pose.drop_location.position.y > 1.0)
{ {
positions = bin1; positions = bin1;
@@ -194,7 +193,6 @@ public:
return; 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); send_joint_trajectory(positions, arm_action_phase::belt_to_bin);
attempts++; 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> 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}; 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) 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; vector<double> cur;
std::stringstream location; std::stringstream location;
location << "Random location: "; location << "Random location: ";
// cur.push_back(y); // 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; int val = rand() % 2;
double diff = 0; double diff = 0;
if (val == 1)
if (val == 1) {
diff = -0.1; diff = -0.1;
else }
else {
diff = 0.1; diff = 0.1;
// double val = (rand() % 70) / 100.0; }
cur.push_back(preferred_state[pos] + diff); cur.push_back(preferred_state[pos] + diff);
location << cur[pos] << " "; location << cur[pos] << " ";
} }
@@ -286,10 +282,9 @@ private:
return cur; 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) 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::Frame desired_end_effector_pose = pose_to_frame(pose);
KDL::JntArray final_joint_configuration(arm->getChain()->getNrOfJoints()); 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 // Convert data attribute (Eigen::VectorXd) of KDL::JntArray to double[] via data() function
int nbr_joints = arm->getChain()->getNrOfJoints(); int nbr_joints = arm->getChain()->getNrOfJoints();
Eigen::VectorXd mat = final_joint_configuration.data; Eigen::VectorXd mat = final_joint_configuration.data;
// cout << mat << endl;
positions.clear(); positions.clear();
// vector<double> positions;
std::stringstream pos_str; std::stringstream pos_str;
pos_str << "Calculated joint positions: "; pos_str << "Calculated joint positions: ";
for (size_t idx = 0; idx < arm->getChain()->getNrOfJoints(); ++idx) for (size_t idx = 0; idx < arm->getChain()->getNrOfJoints(); ++idx)
{ {
positions.push_back(mat[idx]); positions.push_back(mat[idx]);
pos_str << mat[idx] << " "; pos_str << mat[idx] << " ";
} }
pos_str << '\n'; pos_str << '\n';
ROS_INFO(pos_str.str().c_str()); ROS_INFO(pos_str.str().c_str());
@@ -323,6 +318,7 @@ private:
return (error_code == 0); return (error_code == 0);
} }
// Use forward kinematics to calculate end effector pose from current joint states
KDL::Frame calc_end_effector_pose() KDL::Frame calc_end_effector_pose()
{ {
int num_joints = arm->getChain()->getNrOfJoints(); int num_joints = arm->getChain()->getNrOfJoints();
@@ -347,6 +343,7 @@ private:
return end_effector_pose; return end_effector_pose;
} }
// Convert from Pose to KDL::Frame
KDL::Frame pose_to_frame(const geometry_msgs::Pose &pose) KDL::Frame pose_to_frame(const geometry_msgs::Pose &pose)
{ {
double p_x = pose.position.x; double p_x = pose.position.x;
@@ -363,6 +360,7 @@ private:
return KDL::Frame(rot, pos); return KDL::Frame(rot, pos);
} }
// Convert from KDL::Frame to Pose
geometry_msgs::Pose frame_to_pose(const KDL::Frame &frame) geometry_msgs::Pose frame_to_pose(const KDL::Frame &frame)
{ {
double p_x = frame.p.x(); double p_x = frame.p.x();
@@ -386,6 +384,7 @@ private:
return pose; return pose;
} }
// Send desired joint states to joint controller
void send_joint_trajectory(const vector<double> &target, arm_action_phase phase) void send_joint_trajectory(const vector<double> &target, arm_action_phase phase)
{ {
// Declare JointTrajectory message // Declare JointTrajectory message
@@ -448,6 +447,7 @@ private:
ros::Duration((num_points + 1) * time_per_step).sleep(); ros::Duration((num_points + 1) * time_per_step).sleep();
} }
// Turn gripper on or off
bool gripper_control(bool state) bool gripper_control(bool state)
{ {
osrf_gear::VacuumGripperControl srv; osrf_gear::VacuumGripperControl srv;
@@ -463,9 +463,10 @@ private:
return success; 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) bool have_reached_target(geometry_msgs::Pose cur, geometry_msgs::Pose target)
{ {
// Tune threshold values // Threshold values
float pos_thresh = 0.15; // Meters float pos_thresh = 0.15; // Meters
float rot_thresh = 0.05; float rot_thresh = 0.05;
@@ -485,10 +486,10 @@ private:
float qz_err = fabs(fabs(cur.orientation.z) - fabs(target.orientation.z)); float qz_err = fabs(fabs(cur.orientation.z) - fabs(target.orientation.z));
float qw_err = fabs(fabs(cur.orientation.w) - fabs(target.orientation.w)); float qw_err = fabs(fabs(cur.orientation.w) - fabs(target.orientation.w));
ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")"); // ROS_INFO_STREAM("qx_err: " << qx_err << " (" << rot_thresh << ")");
ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")"); // ROS_INFO_STREAM("qy_err: " << qy_err << " (" << rot_thresh << ")");
ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")"); // ROS_INFO_STREAM("qz_err: " << qz_err << " (" << rot_thresh << ")");
ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")"); // ROS_INFO_STREAM("qw_err: " << qw_err << " (" << rot_thresh << ")");
if (qx_err > rot_thresh) if (qx_err > rot_thresh)
{ {
@@ -516,7 +517,7 @@ private:
int main(int argc, char **argv) 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. // Last argument is the default name of the node.
ros::init(argc, argv, "arm_controller_node"); ros::init(argc, argv, "arm_controller_node");
@@ -525,19 +526,19 @@ int main(int argc, char **argv)
ArmRepresentation arm; ArmRepresentation arm;
// Set parameters
double time_per_step = 3.0; // seconds double time_per_step = 3.0; // seconds
int retry_attempts = 3; int retry_attempts = 3;
double item_attach_z_adjustment = 0.05; // meters double item_attach_z_adjustment = 0.05; // meters
ArmController ac(node, &arm, time_per_step, retry_attempts, item_attach_z_adjustment); 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); ros::AsyncSpinner spinner(0);
spinner.start(); 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 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 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::Subscriber joint_state_sub = node.subscribe(ARM_JOINT_STATES_CHANNEL, 1, &ArmController::joint_state_callback, &ac);
ROS_INFO("Setup complete"); ROS_INFO("Setup complete");

View File

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

View File

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

View File

@@ -252,21 +252,7 @@ void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr &cloud_msg)
// publish pose // publish pose
pub.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) int main(int argc, char **argv)

View File

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

View File

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

View File

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

View File

@@ -1,16 +1,4 @@
// Copyright 2016 Open Source Robotics Foundation, Inc. // rrrobot_node.cpp
//
// 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.
#include <algorithm> #include <algorithm>
#include <vector> #include <vector>
@@ -38,8 +26,7 @@
#include "topic_names.h" #include "topic_names.h"
#include "rrrobot/arm_command.h" #include "rrrobot/arm_command.h"
using std::cout; using namespace std;
using std::endl;
class Position class Position
{ {
@@ -78,7 +65,6 @@ public:
desired_drop_pose.position.x = drop_point.x; desired_drop_pose.position.x = drop_point.x;
desired_drop_pose.position.y = drop_point.y; desired_drop_pose.position.y = drop_point.y;
desired_drop_pose.position.z = drop_point.z; desired_drop_pose.position.z = drop_point.z;
// TODO: set the desired orientation of the end effector to point straight down
// update state // update state
if (current_robot_state & RobotState::WAITING_FOR_GRAB_LOCATION) if (current_robot_state & RobotState::WAITING_FOR_GRAB_LOCATION)
@@ -93,7 +79,7 @@ public:
publish_arm_command(); publish_arm_command();
} }
print_state(); // print_state();
} }
void gripper_state_callback(const osrf_gear::VacuumGripperState &state) void gripper_state_callback(const osrf_gear::VacuumGripperState &state)
@@ -117,7 +103,7 @@ public:
// store current state // store current state
gripper_state = state; gripper_state = state;
print_state(); // print_state();
} }
void grasp_pose_callback(const geometry_msgs::Pose &grasp_pose) void grasp_pose_callback(const geometry_msgs::Pose &grasp_pose)
@@ -126,7 +112,7 @@ public:
set_conveyor(0); set_conveyor(0);
desired_grasp_pose = grasp_pose; 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; desired_grasp_pose.position.z += 0.01;
if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION) if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
@@ -140,7 +126,7 @@ public:
publish_arm_command(); publish_arm_command();
} }
print_state(); // print_state();
} }
private: private:
@@ -168,6 +154,7 @@ private:
Position trash_bin = Position(-0.3, 0.383, 1); Position trash_bin = Position(-0.3, 0.383, 1);
Position recycle_bin = Position(-0.3, 1.15, 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 destination(const std::string &type) const
{ {
Position pos; Position pos;
@@ -184,6 +171,7 @@ private:
return pos; return pos;
} }
// Publish message including grab and drop off location for arm controller
void publish_arm_command() void publish_arm_command()
{ {
rrrobot::arm_command cmd; rrrobot::arm_command cmd;
@@ -194,6 +182,7 @@ private:
ros::spinOnce(); ros::spinOnce();
} }
// Set conveyor power (0 or 50-100)
void set_conveyor(int power) void set_conveyor(int power)
{ {
if (power != 0 && (power < 50 || power > 100)) if (power != 0 && (power < 50 || power > 100))
@@ -206,6 +195,7 @@ private:
conveyor_pub.call(cmd); conveyor_pub.call(cmd);
} }
// Print current state for debugging
void print_state() void print_state()
{ {
if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION) if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)

View File

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

View File

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