mirror of
https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git
synced 2025-08-26 09:32:45 +00:00
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:
@@ -1,4 +1,4 @@
|
||||
// arm_controller_node.cpp
|
||||
// arm_representation.h
|
||||
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
@@ -56,4 +56,4 @@ private:
|
||||
|
||||
// KDL::ChainFkSolverPos_recursive fk_solver;
|
||||
// KDL::ChainIkSolverPos_LMA ik_solver;
|
||||
};
|
||||
};
|
||||
|
@@ -1,3 +1,5 @@
|
||||
// topic_names.h
|
||||
|
||||
// COMPUTER VISION
|
||||
#define CV_CLASSIFICATION_CHANNEL "/cv_model" // CV Model classifications
|
||||
|
||||
@@ -13,4 +15,4 @@
|
||||
|
||||
// COMPETITION
|
||||
#define START_COMPETITION_CHANNEL "/ariac/start_competition" // Start ARIAC competition
|
||||
#define CONVEYOR_POWER_CHANNEL "/ariac/conveyor/control" // Turn conveyor belt on or off
|
||||
#define CONVEYOR_POWER_CHANNEL "/ariac/conveyor/control" // Turn conveyor belt on or off
|
||||
|
@@ -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");
|
||||
|
@@ -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,7 +117,8 @@ vector<string> ArmRepresentation::get_joint_names()
|
||||
return joint_names;
|
||||
}
|
||||
|
||||
// Return reference to object
|
||||
KDL::Chain *ArmRepresentation::getChain()
|
||||
{
|
||||
return &chain;
|
||||
}
|
||||
}
|
||||
|
@@ -1,4 +1,6 @@
|
||||
#!/usr/bin/env python
|
||||
# cv_model.py
|
||||
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
@@ -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)
|
||||
@@ -283,4 +269,4 @@ int main(int argc, char **argv)
|
||||
pub = node.advertise<geometry_msgs::Pose>(DESIRED_GRASP_POSE_CHANNEL, 1);
|
||||
|
||||
ros::spin();
|
||||
}
|
||||
}
|
||||
|
@@ -1,4 +1,6 @@
|
||||
#!/usr/bin/env python
|
||||
# image_display.py
|
||||
|
||||
import rospy
|
||||
from std_msgs.msg import String
|
||||
import matplotlib.pyplot as plt
|
||||
@@ -18,4 +20,4 @@ def listener():
|
||||
rospy.spin()
|
||||
|
||||
if __name__ == '__main__':
|
||||
listener()
|
||||
listener()
|
||||
|
@@ -1,3 +1,5 @@
|
||||
// model_insertion_plugin.cpp
|
||||
|
||||
#include <ignition/math/Pose3.hh>
|
||||
#include "gazebo/physics/physics.hh"
|
||||
#include "gazebo/common/common.hh"
|
||||
@@ -71,4 +73,4 @@ private:
|
||||
|
||||
// Register this plugin with the simulator
|
||||
GZ_REGISTER_WORLD_PLUGIN(ModelInsertion)
|
||||
} // namespace gazebo
|
||||
} // namespace gazebo
|
||||
|
@@ -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
|
||||
{
|
||||
@@ -123,4 +124,4 @@ int main(int argc, char **argv)
|
||||
ObjectSpawner spawner;
|
||||
|
||||
ros::spin();
|
||||
}
|
||||
}
|
||||
|
@@ -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,8 +65,7 @@ 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)
|
||||
|
@@ -1,4 +1,4 @@
|
||||
// arm_controller_node.cpp
|
||||
// test_arm.cpp
|
||||
|
||||
#include <algorithm>
|
||||
#include <vector>
|
||||
@@ -106,4 +106,4 @@ int main(int argc, char **argv)
|
||||
cout << "x,y,z: " << end_effector_pose.p.x() << ", " << end_effector_pose.p.y() << ", " << end_effector_pose.p.z() << endl;
|
||||
cout << end_effector_pose.M << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
@@ -1,3 +1,5 @@
|
||||
// test_insert_object.cpp
|
||||
|
||||
#include "rrrobot/model_insertion.h"
|
||||
#include "ros/ros.h"
|
||||
|
||||
@@ -36,4 +38,4 @@ int main(int argc, char **argv)
|
||||
pub.publish(msg);
|
||||
ros::spinOnce();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
Reference in New Issue
Block a user