diff --git a/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp b/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp new file mode 100644 index 0000000..9feb701 --- /dev/null +++ b/rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp @@ -0,0 +1,83 @@ +// arm_controller_node.cpp + +#include +#include +#include + +#include + +#include +#include +#include + + +Class ArmController { +public: + ArmController(ros::NodeHandle & node) : arm_1_has_been_zeroed { + arm_1_joint_trajectory_publisher_ = node.advertise("/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 + +} \ No newline at end of file diff --git a/rrrobot_ws/src/rrrobot/src/cv_model.py b/rrrobot_ws/src/rrrobot/src/cv_model.py new file mode 100644 index 0000000..22b4898 --- /dev/null +++ b/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() diff --git a/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp b/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp new file mode 100644 index 0000000..524855f --- /dev/null +++ b/rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp @@ -0,0 +1,57 @@ +// depth_camera_node.cpp + +#include +#include +#include + +#include +#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 +#include +#include +#include + +#include +#include +#include + + +pcl::PointCloud 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 output = new pcl::PointCloud; + pcl::fromROSMsg(cloud2, output); +} + +void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & msg) { + pcl::PointCloud 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 + +} \ No newline at end of file diff --git a/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp b/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp index 99b82d5..d127f70 100644 --- a/rrrobot_ws/src/rrrobot/src/object_spawner_node.cpp +++ b/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"); diff --git a/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp b/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp index 2314b17..bd60457 100644 --- a/rrrobot_ws/src/rrrobot/src/rrrobot_node.cpp +++ b/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 #include +#include +#include +#include +#include #include @@ -29,31 +31,6 @@ #include #include #include -// %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("/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( "/ariac/arm1/arm/command", 10); - - arm_2_joint_trajectory_publisher_ = node.advertise( - "/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,110 +83,121 @@ 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 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("/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("/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"); + // Last argument is the default name of the node. + ros::init(argc, argv, "rrrobot_node"); - ros::NodeHandle node; + ros::NodeHandle node; - // Instance of custom class from above. - MyCompetitionClass comp_class(node); + // 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/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); - // Subscribe to the '/ariac/competition_state' topic. - ros::Subscriber competition_state_subscriber = node.subscribe( - "/ariac/competition_state", 10, - &MyCompetitionClass::competition_state_callback, &comp_class); + // TODO: Listen to cv_model topic for classification + // ros::Subscriber laser_profiler_subscriber = node.subscribe( + // "/ariac/laser_profiler_1", 10, laser_profiler_callback); - // %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)% + // TODO: Listen to depth camera handler + // - Pass store current object location + // - Make call to arm controller (see below) - // 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: 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::Subscriber arm_2_joint_state_subscriber = node.subscribe( - "/ariac/arm2/joint_states", 10, - &MyCompetitionClass::arm_2_joint_state_callback, &comp_class); + ROS_INFO("Setup complete."); + start_competition(node); + ros::spin(); // This executes callbacks on new data until ctrl-c. - // %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)% - - // 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); - - // 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); - - ROS_INFO("Setup complete."); - start_competition(node); - ros::spin(); // This executes callbacks on new data until ctrl-c. - - return 0; + return 0; } -// %EndTag(MAIN)% -// %EndTag(FULLTEXT)% diff --git a/rrrobot_ws/src/rrrobot/src/v1_0.001.pt b/rrrobot_ws/src/rrrobot/src/v1_0.001.pt new file mode 100644 index 0000000..5c72782 Binary files /dev/null and b/rrrobot_ws/src/rrrobot/src/v1_0.001.pt differ