sravan/RRRobot
1
0
Fork 0
mirror of https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git synced 2025-08-28 18:32:45 +00:00
Code Issues Packages Projects Releases Wiki Activity

main node compiles and has code for all basic functionality required. Currently untested!

Browse Source
This commit is contained in:
Derek Witcpalek
2020-04-23 16:16:35 -04:00
parent 4e522b7088
commit e1b84d4d16
4 changed files with 159 additions and 143 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
src/rrrobot_ws/src/rrrobot/CMakeLists.txt
View File

@@ -25,6 +25,7 @@ find_package(gazebo REQUIRED)
add_message_files(
FILES
model_insertion.msg
arm_command.msg
)
## Generate added messages and services with any dependencies listed here
@@ -45,6 +46,7 @@ catkin_package(
## Build ##
###########
include_directories(
include
${catkin_INCLUDE_DIRS}
${roscpp_INCLUDE_DIRS}
${std_msgs_INCLUDE_DIRS}
@@ -60,7 +62,7 @@ add_dependencies(insert_model ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPOR
## Declare a C++ executable
add_executable(rrrobot_node src/rrrobot_node.cpp)
add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS})
add_dependencies(rrrobot_node ${catkin_EXPORTED_TARGETS} rrrobot_generate_messages_cpp)
target_link_libraries(rrrobot_node ${catkin_LIBRARIES})
add_executable(test_insert_object test/test_insert_object.cpp)

7
src/rrrobot_ws/src/rrrobot/include/topic_names.h Normal file
View File

@@ -0,0 +1,7 @@
#define CV_CLASSIFICATION_CHANNEL "/cv_model"
#define DESIRED_GRASP_POSE_CHANNEL "/desired_grasp_pose" // pose determined from depth camera
#define GRIPPER_STATE_CHANNEL "/ariac/arm1/gripper/state"
#define CONVEYOR_POWER_CHANNEL "/ariac/conveyor/control"
#define START_COMPETITON_CHANNEL "/ariac/start_competition"
#define ARM_DESTINATION_CHANNEL "/arm_controller/destination"
#define START_COMPETITION_CHANNEL "/ariac/start_competition"

2
src/rrrobot_ws/src/rrrobot/msg/arm_command.msg Normal file
View File

@@ -0,0 +1,2 @@
geometry_msgs/Pose grab_location
geometry_msgs/Pose drop_location

263
src/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp
View File

@@ -24,6 +24,8 @@
#include <osrf_gear/LogicalCameraImage.h>
#include <osrf_gear/Order.h>
#include <osrf_gear/Proximity.h>
#include <osrf_gear/VacuumGripperState.h>
#include <osrf_gear/ConveyorBeltControl.h>
#include <sensor_msgs/JointState.h>
#include <sensor_msgs/LaserScan.h>
#include <sensor_msgs/Range.h>
@@ -32,171 +34,174 @@
#include <std_srvs/Trigger.h>
#include <trajectory_msgs/JointTrajectory.h>
/// Example class that can hold state and provide methods that handle incoming data.
class MyCompetitionClass
#include "topic_names.h"
#include "rrrobot/arm_command.h"
class Position
{
public:
explicit MyCompetitionClass(ros::NodeHandle & node)
: current_score_(0), arm_1_has_been_zeroed_(false), arm_2_has_been_zeroed_(false)
float x, y, z;
Position(float x, float y, float z) : x(x), y(y), z(z){};
Position() : x(0), y(0), z(0) {}
};
class RRRobot
{
public:
RRRobot()
: current_robot_state(RobotState::WAITING_FOR_CLASSIFICATION | RobotState::WAITING_FOR_GRAB_LOCATION),
nh()
{
arm_1_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
"/ariac/arm1/arm/command", 10);
cv_classification_sub = nh.subscribe(CV_CLASSIFICATION_CHANNEL, 1000, &RRRobot::cv_classification_callback, this);
gripper_state_sub = nh.subscribe(GRIPPER_STATE_CHANNEL, 1000, &RRRobot::gripper_state_callback, this);
depth_camera_sub = nh.subscribe(DESIRED_GRASP_POSE_CHANNEL, 1000, &RRRobot::grasp_pose_callback, this);
conveyor_pub = nh.serviceClient<osrf_gear::ConveyorBeltControl>(CONVEYOR_POWER_CHANNEL);
arm_destination_pub = nh.advertise<rrrobot::arm_command>(ARM_DESTINATION_CHANNEL, 1000);
// start competition
ros::ServiceClient comp_start = nh.serviceClient<std_srvs::Trigger>(START_COMPETITON_CHANNEL);
std_srvs::Trigger trg;
comp_start.call(trg);
}
/// Called when a new JointState message is received.
void arm_1_joint_state_callback(
const sensor_msgs::JointState::ConstPtr & joint_state_msg)
void cv_classification_callback(const std_msgs::String &classification)
{
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_);
std::string type = classification.data.substr(classification.data.find(":") + 1);
Position drop_point = destination(type);
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)
{
current_robot_state &= ~RobotState::WAITING_FOR_CLASSIFICATION;
}
else
{
current_robot_state = RobotState::MOVING_ARM;
// tell the arm to move to grab the object
publish_arm_command();
}
}
/// 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;
void gripper_state_callback(const osrf_gear::VacuumGripperState &state)
{
if (state.attached)
{
current_robot_state = RobotState::MOVING_ARM;
// 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);
if (!gripper_state.attached)
{
// start the conveyor belt again
set_conveyor(100);
}
}
// just dropped the object
else if (gripper_state.attached /* && !state.attached */)
{
current_robot_state = RobotState::WAITING_FOR_CLASSIFICATION | RobotState::WAITING_FOR_GRAB_LOCATION;
}
// store current state
gripper_state = state;
}
void grasp_pose_callback(const geometry_msgs::Pose &grasp_pose)
{
// stop conveyor belt
set_conveyor(0);
desired_grasp_pose = grasp_pose;
if (current_robot_state & RobotState::WAITING_FOR_CLASSIFICATION)
{
current_robot_state &= ~RobotState::WAITING_FOR_GRAB_LOCATION;
}
else
{
current_robot_state = RobotState::MOVING_ARM;
publish_arm_command();
}
}
private:
ros::Publisher arm_1_joint_trajectory_publisher_;
sensor_msgs::JointState arm_1_current_joint_states_;
bool arm_1_has_been_zeroed_;
};
enum RobotState
{
WAITING_FOR_CLASSIFICATION = 0x1,
WAITING_FOR_GRAB_LOCATION = 0x1 << 1,
MOVING_ARM = 0x1 << 2
};
class Position {
public:
float x, y, z;
int current_robot_state;
osrf_gear::VacuumGripperState gripper_state;
geometry_msgs::Pose desired_grasp_pose;
geometry_msgs::Pose desired_drop_pose;
Position(float x, float y, float z) : x(x), y(y), z(z) {};
};
ros::NodeHandle nh;
Position destination(String type) {
ros::Subscriber cv_classification_sub;
ros::Subscriber gripper_state_sub; // know when item has been grabbed, so conveyor can be started
ros::Subscriber depth_camera_sub; // get desired grab location
ros::ServiceClient conveyor_pub;
ros::Publisher arm_destination_pub;
Position destination(const std::string &type) const
{
Position pos;
float z = 1;
if (type == "cardboard") {
if (type == "cardboard")
pos = Position(-0.3, -1.916, z);
}
else if (type == "glass") {
else if (type == "glass")
pos = Position(-0.3, -1.15, z);
}
else if (type == "metal") {
else if (type == "metal")
pos = Position(-0.3, -0.383, z);
}
else if (type == "paper") {
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 conveyor_controller(ros::NodeHandle node, int power) {
if (power != 0 && (power < 50 || power > 100)) {
void publish_arm_command()
{
rrrobot::arm_command cmd;
cmd.grab_location = desired_grasp_pose;
cmd.drop_location = desired_drop_pose;
arm_destination_pub.publish(cmd);
ros::spinOnce();
}
void set_conveyor(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);
cmd.request.power = power;
conveyor_pub.call(cmd);
}
};
return;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "rrrobot");
// 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");
ros::NodeHandle node;
// Instance of custom class from above.
MyCompetitionClass comp_class(node);
// 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);
// TODO: Spawn items (random order)
// - Make a call to gazebo model insert script
spawn_items(node);
// TODO: Listen to cv_model topic for classification
// ros::Subscriber laser_profiler_subscriber = node.subscribe(
// "/ariac/laser_profiler_1", 10, laser_profiler_callback);
// TODO: Listen to depth camera handler
// - Pass store current object location
// - Make call to arm controller (see below)
// 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);
RRRobot robot;
ros::spin(); // This executes callbacks on new data until ctrl-c.
return 0;