mirror of
https://github.com/EECS-467-W20-RRRobot-Project/RRRobot.git
synced 2025-08-30 11:03:13 +00:00
Full Code Framework
- Add node for controlling arm - Add node for cv model - Add trained cv model data - Add node for handling depth camera - Add node for spawning in new items - Add main node to handle interactions with other nodes
This commit is contained in:
83
rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
Normal file
83
rrrobot_ws/src/rrrobot/src/arm_controller_node.cpp
Normal file
@@ -0,0 +1,83 @@
|
|||||||
|
// arm_controller_node.cpp
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <vector>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
|
||||||
|
#include <std_msgs/Float32.h>
|
||||||
|
#include <std_msgs/String.h>
|
||||||
|
#include <std_srvs/Trigger.h>
|
||||||
|
|
||||||
|
|
||||||
|
Class ArmController {
|
||||||
|
public:
|
||||||
|
ArmController(ros::NodeHandle & node) : arm_1_has_been_zeroed {
|
||||||
|
arm_1_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>("/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
|
||||||
|
|
||||||
|
}
|
77
rrrobot_ws/src/rrrobot/src/cv_model.py
Normal file
77
rrrobot_ws/src/rrrobot/src/cv_model.py
Normal file
@@ -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()
|
57
rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp
Normal file
57
rrrobot_ws/src/rrrobot/src/depth_camera_node.cpp
Normal file
@@ -0,0 +1,57 @@
|
|||||||
|
// depth_camera_node.cpp
|
||||||
|
|
||||||
|
#include <algorithm>
|
||||||
|
#include <vector>
|
||||||
|
#include <string>
|
||||||
|
|
||||||
|
#include <ros/ros.h>
|
||||||
|
#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 <pcl_conversions/pcl_conversions.h>
|
||||||
|
#include <pcl/point_cloud.h>
|
||||||
|
#include <pcl/point_types.h>
|
||||||
|
#include <pcl/segmentation/extract_clusters.h>
|
||||||
|
|
||||||
|
#include <std_msgs/Float32.h>
|
||||||
|
#include <std_msgs/String.h>
|
||||||
|
#include <std_srvs/Trigger.h>
|
||||||
|
|
||||||
|
|
||||||
|
pcl::PointCloud<pcl::PointXYZ> 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<pcl::PointXYZ> output = new pcl::PointCloud<pcl::PointXYZ>;
|
||||||
|
pcl::fromROSMsg(cloud2, output);
|
||||||
|
}
|
||||||
|
|
||||||
|
void depth_camera_callback(const sensor_msgs::PointCloud::ConstPtr & msg) {
|
||||||
|
pcl::PointCloud<pcl::PointXYZ> 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
|
||||||
|
|
||||||
|
}
|
@@ -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)
|
int main(int argc, char **argv)
|
||||||
{
|
{
|
||||||
ros::init(argc, argv, "object_spawner");
|
ros::init(argc, argv, "object_spawner");
|
||||||
|
@@ -12,10 +12,12 @@
|
|||||||
// See the License for the specific language governing permissions and
|
// See the License for the specific language governing permissions and
|
||||||
// limitations under the License.
|
// limitations under the License.
|
||||||
|
|
||||||
// %Tag(FULLTEXT)%
|
|
||||||
// %Tag(INCLUDE_STATEMENTS)%
|
|
||||||
#include <algorithm>
|
#include <algorithm>
|
||||||
#include <vector>
|
#include <vector>
|
||||||
|
#include <string>
|
||||||
|
#include <stdlib.h>
|
||||||
|
#include <time.h>
|
||||||
|
#include <fstream>
|
||||||
|
|
||||||
#include <ros/ros.h>
|
#include <ros/ros.h>
|
||||||
|
|
||||||
@@ -29,31 +31,6 @@
|
|||||||
#include <std_msgs/String.h>
|
#include <std_msgs/String.h>
|
||||||
#include <std_srvs/Trigger.h>
|
#include <std_srvs/Trigger.h>
|
||||||
#include <trajectory_msgs/JointTrajectory.h>
|
#include <trajectory_msgs/JointTrajectory.h>
|
||||||
// %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<std_srvs::Trigger>("/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.
|
/// Example class that can hold state and provide methods that handle incoming data.
|
||||||
class MyCompetitionClass
|
class MyCompetitionClass
|
||||||
@@ -62,40 +39,10 @@ public:
|
|||||||
explicit MyCompetitionClass(ros::NodeHandle & node)
|
explicit MyCompetitionClass(ros::NodeHandle & node)
|
||||||
: current_score_(0), arm_1_has_been_zeroed_(false), arm_2_has_been_zeroed_(false)
|
: 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<trajectory_msgs::JointTrajectory>(
|
arm_1_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
|
||||||
"/ariac/arm1/arm/command", 10);
|
"/ariac/arm1/arm/command", 10);
|
||||||
|
|
||||||
arm_2_joint_trajectory_publisher_ = node.advertise<trajectory_msgs::JointTrajectory>(
|
|
||||||
"/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.
|
/// Called when a new JointState message is received.
|
||||||
void arm_1_joint_state_callback(
|
void arm_1_joint_state_callback(
|
||||||
const sensor_msgs::JointState::ConstPtr & joint_state_msg)
|
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.
|
/// Create a JointTrajectory with all positions set to zero, and command the arm.
|
||||||
void send_arm_to_zero_state(ros::Publisher & joint_trajectory_publisher) {
|
void send_arm_to_zero_state(ros::Publisher & joint_trajectory_publisher) {
|
||||||
// Create a message to send.
|
// Create a message to send.
|
||||||
@@ -152,110 +83,121 @@ public:
|
|||||||
ROS_INFO_STREAM("Sending command:\n" << msg);
|
ROS_INFO_STREAM("Sending command:\n" << msg);
|
||||||
joint_trajectory_publisher.publish(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:
|
private:
|
||||||
std::string competition_state_;
|
|
||||||
double current_score_;
|
|
||||||
ros::Publisher arm_1_joint_trajectory_publisher_;
|
ros::Publisher arm_1_joint_trajectory_publisher_;
|
||||||
ros::Publisher arm_2_joint_trajectory_publisher_;
|
|
||||||
std::vector<osrf_gear::Order> received_orders_;
|
|
||||||
sensor_msgs::JointState arm_1_current_joint_states_;
|
sensor_msgs::JointState arm_1_current_joint_states_;
|
||||||
sensor_msgs::JointState arm_2_current_joint_states_;
|
|
||||||
bool arm_1_has_been_zeroed_;
|
bool arm_1_has_been_zeroed_;
|
||||||
bool arm_2_has_been_zeroed_;
|
|
||||||
};
|
};
|
||||||
|
|
||||||
void proximity_sensor_callback(const sensor_msgs::Range::ConstPtr & msg) {
|
class Position {
|
||||||
if ((msg->max_range - msg->range) > 0.01) { // If there is an object in proximity.
|
public:
|
||||||
ROS_INFO_THROTTLE(1, "Proximity sensor sees something.");
|
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) {
|
void conveyor_controller(ros::NodeHandle node, int power) {
|
||||||
size_t number_of_valid_ranges = std::count_if(
|
if (power != 0 && (power < 50 || power > 100)) {
|
||||||
msg->ranges.begin(), msg->ranges.end(), [](const float f) {return std::isfinite(f);});
|
return;
|
||||||
if (number_of_valid_ranges > 0) {
|
}
|
||||||
ROS_INFO_THROTTLE(1, "Laser profiler sees something.");
|
|
||||||
}
|
ros::ServiceClient client = node.serviceClient<osrf_gear::ConveyorBeltControl>("/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<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) {
|
int main(int argc, char ** argv) {
|
||||||
// Last argument is the default name of the node.
|
// Last argument is the default name of the node.
|
||||||
ros::init(argc, argv, "rrrobot_node");
|
ros::init(argc, argv, "rrrobot_node");
|
||||||
|
|
||||||
ros::NodeHandle node;
|
ros::NodeHandle node;
|
||||||
|
|
||||||
// Instance of custom class from above.
|
// Instance of custom class from above.
|
||||||
MyCompetitionClass comp_class(node);
|
MyCompetitionClass comp_class(node);
|
||||||
|
|
||||||
// Subscribe to the '/ariac/current_score' topic.
|
// Subscribe to the '/ariac/joint_states' topic.
|
||||||
ros::Subscriber current_score_subscriber = node.subscribe(
|
ros::Subscriber arm_1_joint_state_subscriber = node.subscribe(
|
||||||
"/ariac/current_score", 10,
|
"/ariac/arm1/joint_states", 10,
|
||||||
&MyCompetitionClass::current_score_callback, &comp_class);
|
&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.
|
// TODO: Listen to cv_model topic for classification
|
||||||
ros::Subscriber competition_state_subscriber = node.subscribe(
|
// ros::Subscriber laser_profiler_subscriber = node.subscribe(
|
||||||
"/ariac/competition_state", 10,
|
// "/ariac/laser_profiler_1", 10, laser_profiler_callback);
|
||||||
&MyCompetitionClass::competition_state_callback, &comp_class);
|
|
||||||
|
|
||||||
// %Tag(SUB_CLASS)%
|
// TODO: Listen to depth camera handler
|
||||||
// Subscribe to the '/ariac/orders' topic.
|
// - Pass store current object location
|
||||||
ros::Subscriber orders_subscriber = node.subscribe(
|
// - Make call to arm controller (see below)
|
||||||
"/ariac/orders", 10,
|
|
||||||
&MyCompetitionClass::order_callback, &comp_class);
|
|
||||||
// %EndTag(SUB_CLASS)%
|
|
||||||
|
|
||||||
// Subscribe to the '/ariac/joint_states' topic.
|
// TODO: Start arm controller
|
||||||
ros::Subscriber arm_1_joint_state_subscriber = node.subscribe(
|
// - Stop conveyor belt
|
||||||
"/ariac/arm1/joint_states", 10,
|
// - Send destination and current location to arm controller
|
||||||
&MyCompetitionClass::arm_1_joint_state_callback, &comp_class);
|
// - Wait to receive successful drop-off signal
|
||||||
|
// - Start conveyor belt
|
||||||
|
// - Spawn new items
|
||||||
|
|
||||||
ros::Subscriber arm_2_joint_state_subscriber = node.subscribe(
|
ROS_INFO("Setup complete.");
|
||||||
"/ariac/arm2/joint_states", 10,
|
start_competition(node);
|
||||||
&MyCompetitionClass::arm_2_joint_state_callback, &comp_class);
|
ros::spin(); // This executes callbacks on new data until ctrl-c.
|
||||||
|
|
||||||
// %Tag(SUB_FUNC)%
|
return 0;
|
||||||
// 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;
|
|
||||||
}
|
}
|
||||||
// %EndTag(MAIN)%
|
|
||||||
// %EndTag(FULLTEXT)%
|
|
||||||
|
BIN
rrrobot_ws/src/rrrobot/src/v1_0.001.pt
Normal file
BIN
rrrobot_ws/src/rrrobot/src/v1_0.001.pt
Normal file
Binary file not shown.
Reference in New Issue
Block a user