void Listener::ros_comms_init() {
ros::NodeHandle n;
image_transport::ImageTransport it_(n);
image_sub_ = it_.subscribe("/openni2_camera/rgb/image_raw", 1,
&Listener::chatterCallback, this);
//chatter_subscriber = n.subscribe("chatter", 1000, &Listener::chatterCallback, this);
}
Node ()
// initialization list copy the arguments to the class attributes.
{
imageOut_.init(480,640);
lastHeaderSeq_ = 1;
image_transport::ImageTransport it_(nh_);
//pub_ = it_.advertise("out", 1);
sub_ = it_.subscribe("vrep/Vision_sensorIT", 1, &Node::subscriberCb,this);
// create subscriber, publisher using nh_.
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "edp2_c4");
ros::NodeHandle nh_;
image_transport::ImageTransport it_(nh_);
image_transport::Subscriber image_sub_;
image_sub_ = it_.subscribe("/camera/rgb/image_color", 1, imageCb); //subscribes to the Kinect video frames
ros::spin();
return 0;
}
Node (ros::NodeHandle& nh, ros::NodeHandle& nhPrivate) :
nh_(*nh),
nhPrivate_(*nhPrivate)
// initialization list copy the arguments to the class attributes.
{
//nh_ = nh;
//nhPrivate_=nhPrivate;
//it_ = *it;
it_(nh_);
imageOut_.init(480,640);
lastHeaderSeq_ = 1;
//pub_ = it_.advertise("out", 1);
sub_ = it_.subscribe("vrep/Vision_sensorIT", 1, &Node::subscriberCb,this);
// create subscriber, publisher using nh_.
}
int main(int argc, char **argv)
{
// unit test for extractPose() and createHomogeneousTransformMatrix()
//~ tf::StampedTransform t;
//~ tf::Vector3 origin(2, 3, 4);
//~ tf::Quaternion quaternion(0, 1, 0, 1);
//~ t.setOrigin(origin);
//~ t.setRotation(quaternion);
//~ geometry_msgs::Pose p = convertToPose(t);
//~ ROS_INFO_STREAM("pose:\n" << p);
//~ Eigen::Matrix4d T = createHomogeneousTransformMatrix(t);
//~ ROS_INFO_STREAM("homogeneous transformation matrix:\n" << T);
//~ return 1;
ros::init(argc, argv, "insertion");
ros::NodeHandle node("~");
ros::AsyncSpinner spinner(1);
spinner.start();
/*
ros::Publisher pub = node.advertise<baxter_core_msgs::JointCommand>("/robot/limb/right/joint_command", 10);
ros::Subscriber sub = node.subscribe("/usb_pose", 10, usbCallback);
ros::Rate rate(1.0);
ros::Publisher gripper_pub = node.advertise<baxter_core_msgs::EndEffectorCommand>("/robot/end_effector/right_gripper/command", 10);
*/
// create OpenCV window
cv::namedWindow(OPENCV_WINDOW);
cv::namedWindow(OPENCV_WINDOW_K, CV_GUI_NORMAL);
node.param<std::string>("camera_topic", camera_topic, "/cameras/right_hand_camera/image");
ROS_INFO("Listening to: %s", camera_topic.c_str());
// subscribe to the camera topic
image_transport::ImageTransport it_(node);
image_transport::Subscriber image_sub_ = it_.subscribe(camera_topic, 1,
imageCallback);
// move arm top in front of wires
// once arm is in position, ready to detect blobs
readyToFindWire = true;
/*
// create messages to control the gripper
baxter_core_msgs::EndEffectorCommand ee_msg_close;
ee_msg_close.id = 65664;
ee_msg_close.command = "grip";
ee_msg_close.args = "{}";
baxter_core_msgs::EndEffectorCommand ee_msg_open;
ee_msg_open.id = 65664;
ee_msg_open.command = "release";
ee_msg_open.args = "{}";
move_group_interface::MoveGroup group("right_arm");
move_group_interface::MoveGroup group1("right_arm");
move_group_interface::MoveGroup group2("right_arm");
move_group_interface::MoveGroup group3("right_arm");
move_group_interface::MoveGroup group4("right_arm");
move_group_interface::MoveGroup group5("right_arm");
move_group_interface::MoveGroup group6("right_arm");
move_group_interface::MoveGroup group7("right_arm");
// get current pose and joints
geometry_msgs::PoseStamped curr_pose = group.getCurrentPose();
ROS_INFO_STREAM("current end effector pose: "<<curr_pose);
std::vector<double> joint_values = group.getCurrentJointValues();
std::vector<std::string> joint_names = group.getJoints();
for (int i = 0; i < joint_values.size(); i++)
std::cout<<joint_names[i]<<": "<<joint_values.at(i)<<std::endl;
// setup IK solver
ros::ServiceClient service_client = node.serviceClient<moveit_msgs::GetPositionIK>("/compute_ik");
while(!service_client.exists())
{
ROS_INFO("Waiting for service");
sleep(1.0);
}
moveit_msgs::GetPositionIK::Request service_request;
moveit_msgs::GetPositionIK::Response service_response;
ROS_INFO_STREAM("Press Enter when ready ");
char c = std::cin.get();
curr_pose = group.getCurrentPose();
ROS_INFO_STREAM("current end effector pose: "<<curr_pose);
// hardcode fixed target poses
geometry_msgs::PoseStamped p1, p2, p3, p4, p5, p6, p7, p8, p9;
p1.header = createHeader(ros::Time(0), "/base");
p1.pose.position = createPoint(0.76, -0.445, -0.02);
p1.pose.orientation = createQuaternion(0,0.707,0,0.707);
p2.header = createHeader(ros::Time(0), "/base");
p2.pose.position = createPoint(0.815, -0.445, -0.025);
p2.pose.orientation = createQuaternion(0,0.707,0,0.707);
p3.header = createHeader(ros::Time(0), "/base");
p3.pose.position = createPoint(0.80, -0.445, -0.025);
p3.pose.orientation = createQuaternion(0,0.707,0,0.707);
p4.header = createHeader(ros::Time(0), "/base");
p4.pose.position = createPoint(0.83, -0.445, -0.025);
p4.pose.orientation = createQuaternion(0,0.707,0,0.707);
p5.header = createHeader(ros::Time(0), "/base");
p5.pose.position = createPoint(0.835, -0.445, -0.025);
p5.pose.orientation = createQuaternion(0,0.707,0,0.707);
p6.header = createHeader(ros::Time(0), "/base");
p6.pose.position = createPoint(0.815, -0.445, -0.095);
p6.pose.orientation = createQuaternion(0,0.707,0,0.707);
p7.header = createHeader(ros::Time(0), "/base");
p7.pose.position = createPoint(0.815, -0.445, -0.115);
p7.pose.orientation = createQuaternion(0,0.707,0,0.707);
p8.header = createHeader(ros::Time(0), "/base");
p8.pose.position = createPoint(0.819, -0.445, -0.115);
p8.pose.orientation = createQuaternion(0,0.707,0,0.707);
p9.header = createHeader(ros::Time(0), "/base");
p9.pose.position = createPoint(0.811, -0.445, -0.115);
p9.pose.orientation = createQuaternion(0,0.707,0,0.707);
std::vector<geometry_msgs::PoseStamped> waypoints;
waypoints.push_back(p1);
waypoints.push_back(p2);
waypoints.push_back(p3);
waypoints.push_back(p4);
waypoints.push_back(p5);
waypoints.push_back(p6);
waypoints.push_back(p7);
waypoints.push_back(p8);
// ---------------------------------------------------------
// hardcode nominal joint configuration
std::vector<double> joint_values_nominal = group.getCurrentJointValues();
std::vector<double> joint_values_pregrasp = group.getCurrentJointValues();
// These values are for the USB-Monitor alignment
joint_values_nominal[0] = 1.04963;
joint_values_nominal[1] = -0.865932;
joint_values_nominal[2] = -0.0613592;
joint_values_nominal[3] = 2.2864;
joint_values_nominal[4] = 1.055;
joint_values_nominal[5] = 0.00076699;
joint_values_nominal[6] = -0.885107;
//~ joint_values_nominal[0] = 0.627015;
//~ joint_values_nominal[1] = -0.575626;
//~ joint_values_nominal[2] = 0.883189;
//~ joint_values_nominal[3] = 1.50982;
//~ joint_values_nominal[4] = -0.854044;
//~ joint_values_nominal[5] = 0.967942;
//~ joint_values_nominal[6] = 2.38419;
// joint values for pregrasp of USB
//~ joint_values_pregrasp[0] = 0.0318301;
//~ joint_values_pregrasp[1] = 0.0644272;
//~ joint_values_pregrasp[2] = 0.37851;
//~ joint_values_pregrasp[3] = 1.42468;
//~ joint_values_pregrasp[4] = -0.790767;
//~ joint_values_pregrasp[5] = -1.24828;
//~ joint_values_pregrasp[6] = -0.179859;
joint_values_pregrasp[0] = 0.530374;
joint_values_pregrasp[1] = -0.368539;
joint_values_pregrasp[2] = 0.214374;
joint_values_pregrasp[3] = 2.24345;
joint_values_pregrasp[4] = 0.274583;
joint_values_pregrasp[5] = -1.43005;
joint_values_pregrasp[6] = -0.158767;
MoveGroupPlan plan;
//~ // ---------------------------------------------------------
//~ // Move to nominal configuration
//~ joint_values = group.getCurrentJointValues();
//~ ROS_INFO_STREAM("Press Enter to move to nominal configuration");
//~ c = std::cin.get();
//~ plan = createPlan(joint_values, joint_values_nominal, 10, 1, joint_names);
//~ if (c == '\n')
//~ {
//~ plan = planStampNow(plan);
//~ group.execute(plan);
//~ }
//~ ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
// ---------------------------------------------------------
// Move to pre-grasp configuration
joint_values = group.getCurrentJointValues();
ROS_INFO_STREAM("Press Enter to move to pregrasp configuration");
c = std::cin.get();
plan = createPlan(joint_values, joint_values_pregrasp, 10, 1, joint_names);
if (c == '\n')
{
plan = planStampNow(plan);
group1.execute(plan);
}
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
std::vector<double> target_joint_values(joint_names.size());
sensor_msgs::JointState joint_state;
//~ // solve IK for each waypoint in list
int service_error;
std::vector<sensor_msgs::JointState> waypoints_joint;
service_request.ik_request.group_name = "right_arm";
for (int i=0; i<waypoints.size(); i++)
{
service_request.ik_request.pose_stamped = waypoints[i];
service_client.call(service_request, service_response);
ROS_INFO_STREAM("waypoints["<<0<<"]: \n"<<waypoints[i]);
service_error = service_response.error_code.val;
ROS_INFO_STREAM("IK solver error code: " << service_error);
if (service_error == service_response.error_code.FAILURE)
{
ROS_INFO_STREAM("Exiting ...");
return 1;
}
waypoints_joint.push_back(service_response.solution.joint_state);
}
// ---------------------------------------------------------
// Move to waypoint 1
joint_values = group1.getCurrentJointValues();
joint_state = waypoints_joint[0];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Press Enter to move to target #"<<1);
c = std::cin.get();
if (c == '\n')
{
move(pub, group1, target_joint_values, joint_names,0.08,0.01);
}
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
// ---------------------------------------------------------
// Move to waypoint 2
joint_values = group2.getCurrentJointValues();
joint_state = waypoints_joint[1];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Moving to target #"<<2);
move(pub, group2, target_joint_values, joint_names,0.08,0.002);
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
// ---------------------------------------------------------
// Iteratively close gripper until we find the USB
move_group_interface::MoveGroup * grouptemp;
//~ move_group_interface::MoveGroup grouptemp("right_arm");
for (int usbiter = 0; usbiter<4; usbiter++)
{
for (int usbiter2 = 0; usbiter2<2; usbiter2++)
{
grouptemp = new move_group_interface::MoveGroup("right_arm");
joint_values = grouptemp->getCurrentJointValues();
joint_state = waypoints_joint[1+usbiter2];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Moving to target #"<<2+usbiter2);
move(pub, *grouptemp, target_joint_values, joint_names,0.08,0.002);
ROS_INFO_STREAM("Press Enter to close gripper"<<0);
c = std::cin.get();
usbFound = 0; // zero out flag
if (c == '\n')
{
gripper_pub.publish(ee_msg_close);
}
usleep(4000000); // sleep 1 sec
ROS_INFO_STREAM("hi 1");
//~ delete grouptemp;
ROS_INFO_STREAM("hi 2");
if (usbFound > 0) {
ROS_INFO_STREAM("hi 3");
usbiter = 10; // cause outer loop to break
break;
}
gripper_pub.publish(ee_msg_open);
}
}
ROS_INFO_STREAM("hi 4");
//~ ROS_INFO_STREAM("Close gripper. Press Enter when done"<<0);
//~ c = std::cin.get();
//~ std::vector<double> joint_values_preinsert = group.getCurrentJointValues();
//~ joint_values_preinsert[0] = 0.737845;
//~ joint_values_preinsert[1] = 0.0682621;
//~ joint_values_preinsert[2] = 0.266913;
//~ joint_values_preinsert[3] = 1.54012;
//~ joint_values_preinsert[4] = -1.66552;
//~ joint_values_preinsert[5] = -1.28624;
//~ joint_values_preinsert[6] = -0.0705631;
//~
//~ // ---------------------------------------------------------
//~ // Move to pre-insert configuration
//~ joint_values = group.getCurrentJointValues();
//~ ROS_INFO_STREAM("Press Enter to move to pregrasp configuration");
//~ c = std::cin.get();
//~ plan = createPlan(joint_values, joint_values_preinsert, 10, 1, joint_names);
//~ if (c == '\n')
//~ {
//~ plan = planStampNow(plan);
//~ group.execute(plan);
//~ }
//~ ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
//~
//~
// ---------------------------------------------------------
// Move to waypoint 5
joint_values = group3.getCurrentJointValues();
joint_state = waypoints_joint[4];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Press Enter to move to target #"<<5);
c = std::cin.get();
//~ plan = createPlan(joint_values, target_joint_values, 5, 1, joint_names);
if (c == '\n')
{
move(pub, group3, target_joint_values, joint_names,0.08,0.01);
}
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
// ---------------------------------------------------------
// Move to waypoint 6
joint_values = group4.getCurrentJointValues();
joint_state = waypoints_joint[5];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Press Enter to move to target #"<<6);
c = std::cin.get();
if (c == '\n')
{
move(pub, group4, target_joint_values, joint_names,0.04,0.004);
}
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
printf("usbFound: %d\n", usbFound);
printf("usbPose: %f, %f, %f\n", usbPose.position.x, usbPose.position.y, usbPose.position.z);
// These are the delta motions to send to the gripper
//~ double deltaz = -0.001*(usbPose.position.x - 90)/22;
//~ double deltax = 0.001*(usbPose.position.y + 78)/22;
double deltaz = -0.001*(usbPose.position.x - 90)/18;
double deltax = 0.001*(usbPose.position.y + 78)/18;
deltax -= 0.001*(usbPose.orientation.w+1)*0.55;
ROS_INFO_STREAM("Gripper offset from sensor: "<<deltax<<", "<<deltaz);
waypoints[6].pose.position.x = waypoints[6].pose.position.x + deltax;
waypoints[6].pose.position.z = waypoints[6].pose.position.z + deltaz;
// solve IK for each waypoint in list
waypoints_joint.clear();
service_request.ik_request.group_name = "right_arm";
for (int i=0; i<waypoints.size(); i++)
{
service_request.ik_request.pose_stamped = waypoints[i];
service_client.call(service_request, service_response);
ROS_INFO_STREAM("waypoints["<<0<<"]: \n"<<waypoints[i]);
service_error = service_response.error_code.val;
ROS_INFO_STREAM("IK solver error code: " << service_error);
if (service_error == service_response.error_code.FAILURE)
{
ROS_INFO_STREAM("Exiting ...");
return 1;
}
waypoints_joint.push_back(service_response.solution.joint_state);
}
// ---------------------------------------------------------
// Move to waypoint 7
joint_values = group.getCurrentJointValues();
joint_state = waypoints_joint[6];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Press Enter to move to target #"<<7);
c = std::cin.get();
if (c == '\n')
{
move(pub, group5, target_joint_values, joint_names,0.02,0.002);
}
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
// ---------------------------------------------------------
// Move to waypoint 8
joint_values = group.getCurrentJointValues();
joint_state = waypoints_joint[7];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Press Enter to move to target #"<<8);
move(pub, group5, target_joint_values, joint_names,0.02,0.004);
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
// ---------------------------------------------------------
// Move to waypoint 9
joint_values = group.getCurrentJointValues();
joint_state = waypoints_joint[8];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Press Enter to move to target #"<<9);
move(pub, group6, target_joint_values, joint_names,0.02,0.004);
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
// ---------------------------------------------------------
// Move to waypoint 7
joint_values = group.getCurrentJointValues();
joint_state = waypoints_joint[6];
target_joint_values = extractRightArmJointValues(joint_state);
ROS_INFO_STREAM("Press Enter to move to target #"<<7);
move(pub, group7, target_joint_values, joint_names,0.02,0.004);
ROS_INFO_STREAM("current end-effector pose: "<<group.getCurrentPose());
ROS_INFO_STREAM("press any key to terminate");
c = std::cin.get();
*/
// destroy cv window
ROS_INFO_STREAM("press any key to terminate");
std::cin.get();
cv::destroyWindow(OPENCV_WINDOW);
cv::destroyWindow(OPENCV_WINDOW_K);
ros::shutdown();
return 0;
}
