int main(int argc, char** argv) {
ros::init(argc, argv, "example_cart_move_action_client"); // name this node
ros::NodeHandle nh; //standard ros node handle
ArmMotionCommander arm_motion_commander(&nh);
Eigen::VectorXd right_arm_joint_angles;
Eigen::Vector3d dp_displacement;
int rtn_val;
geometry_msgs::PoseStamped rt_tool_pose;
FurtherUpdatedPclUtils cwru_pcl_utils(&nh);
while (!cwru_pcl_utils.got_kinect_cloud())
{
ROS_INFO("did not receive pointcloud");
ros::spinOnce();
ros::Duration(1.0).sleep();
}
ROS_INFO("got a pointcloud");
tf::StampedTransform tf_sensor_frame_to_torso_frame; // use this to transform sensor frame to torso frame
tf::TransformListener tf_listener; // start a transform listener
// let's warm up the tf_listener, to make sure it get's all the transforms it needs to avoid crashing:
bool tferr = true;
ROS_INFO("waiting for tf between kinect_pc_frame and torso...");
while (tferr)
{
tferr = false;
try
{
// The direction of the transform returned will be from the target_frame to the source_frame.
// Which if applied to data, will transform data in the source_frame into the target_frame.
// See tf/CoordinateFrameConventions#Transform_Direction
tf_listener.lookupTransform("torso", "kinect_pc_frame", ros::Time(0), tf_sensor_frame_to_torso_frame);
}
catch (tf::TransformException &exception)
{
ROS_ERROR("%s", exception.what());
tferr = true;
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
}
ROS_INFO("tf is good"); // tf-listener found a complete chain from sensor to world; ready to roll
// convert the tf to an Eigen::Affine:
Eigen::Affine3f A_sensor_wrt_torso;
A_sensor_wrt_torso = cwru_pcl_utils.transformTFToEigen(tf_sensor_frame_to_torso_frame);
// transform the kinect data to the torso frame;
// we don't need to have it returned; cwru_pcl_utils can own it as a member var
while (ros::ok())
{
ROS_INFO("Waiting for selected points");
if (cwru_pcl_utils.got_selected_points())
{
//get tool pose
cwru_pcl_utils.transform_selected_points_cloud(A_sensor_wrt_torso);
rtn_val = arm_motion_commander.rt_arm_request_tool_pose_wrt_torso();
rt_tool_pose = arm_motion_commander.get_rt_tool_pose_stamped();
//alter the tool pose:
cwru_pcl_utils.find_selected_centroid(rt_tool_pose);
ROS_INFO("Returned centroid is at (%f, %f, %f)",
rt_tool_pose.pose.position.x, rt_tool_pose.pose.position.y, rt_tool_pose.pose.position.z);
// Set z frame to table height, for some reason this doesn't line up as it should?
rt_tool_pose.pose.position.z -= .08;
// send move plan request:
ROS_INFO("Changed z value to %f", rt_tool_pose.pose.position.z);
rtn_val=arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose);
//send command to execute planned motion
rtn_val=arm_motion_commander.rt_arm_execute_planned_path();
rtn_val = arm_motion_commander.rt_arm_request_tool_pose_wrt_torso();
// Do wiping motion
rt_tool_pose.pose.position.y -= .2;
rtn_val=arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose);
//send command to execute planned motion
rtn_val=arm_motion_commander.rt_arm_execute_planned_path();
rt_tool_pose.pose.position.y += .4;
rtn_val=arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose);
//send command to execute planned motion
rtn_val=arm_motion_commander.rt_arm_execute_planned_path();
cwru_pcl_utils.reset_got_selected_points();
}
ros::Duration(1).sleep();
ros::spinOnce();
}
return 0;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "vision_guided_motion_action_client");
ros::NodeHandle nh;
// create an instance of arm commander class and cwru_pcl_utils class
ArmMotionCommander arm_motion_commander(&nh);
CwruPclUtils cwru_pcl_utils(&nh);
// wait to receive point cloud
while (!cwru_pcl_utils.got_kinect_cloud())
{
ROS_INFO("did not receive pointcloud");
ros::spinOnce();
ros::Duration(1.0).sleep();
}
ROS_INFO("got a pointcloud");
tf::StampedTransform tf_sensor_frame_to_torso_frame; // use this to transform sensor frame to torso frame
tf::TransformListener tf_listener; // start a transform listener
// warm up the tf_listener (this is to make sure it get's all the transforms it needs to avoid crashing)
bool tferr = true;
ROS_INFO("waiting for tf between kinect_pc_frame and torso...");
while (tferr)
{
tferr = false;
try
{
// The direction of the transform returned will be from the target_frame to the source_frame
// which if applied to data, will transform data in the source_frame into the target_frame
// See tf/CoordinateFrameConventions#Transform_Direction
tf_listener.lookupTransform("torso", "kinect_pc_frame", ros::Time(0), tf_sensor_frame_to_torso_frame);
}
catch (tf::TransformException &exception)
{
ROS_ERROR("%s", exception.what());
tferr = true;
ros::Duration(0.5).sleep();
ros::spinOnce();
}
}
ROS_INFO("tf is good"); // tf_listener found a complete chain from sensor to world
Eigen::Affine3f A_sensor_wrt_torso;
Eigen::Affine3d Affine_des_gripper;
Eigen::Vector3d xvec_des, yvec_des, zvec_des, origin_des;
geometry_msgs::PoseStamped rt_tool_pose;
Eigen::Vector3f plane_normal, major_axis, centroid;
Eigen::Matrix3d Rmat;
int rtn_val;
double plane_dist;
// convert the tf to an Eigen::Affine
A_sensor_wrt_torso = cwru_pcl_utils.transformTFToEigen(tf_sensor_frame_to_torso_frame);
// send a command to plan and then execute a joint-space move to pre-defined pose
rtn_val = arm_motion_commander.plan_move_to_pre_pose();
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
// repeatedly scan for a new patch of selected points
while (ros::ok())
{
if (cwru_pcl_utils.got_selected_points())
{
ROS_INFO("transforming selected points");
cwru_pcl_utils.transform_selected_points_cloud(A_sensor_wrt_torso);
// fit a plane to these selected points
cwru_pcl_utils.fit_xformed_selected_pts_to_plane(plane_normal, plane_dist);
ROS_INFO_STREAM("normal: " << plane_normal.transpose() << "\ndist = " << plane_dist);
major_axis = cwru_pcl_utils.get_major_axis();
centroid = cwru_pcl_utils.get_centroid();
cwru_pcl_utils.reset_got_selected_points(); // reset the selected-points trigger
// construct a goal affine pose
for (int i = 0; i < 3; i++) {
origin_des[i] = centroid[i]; // convert to double precision
zvec_des[i] = -plane_normal[i]; // want tool z pointing OPPOSITE surface normal
xvec_des[i] = major_axis[i];
}
origin_des[2] += 0.02; // raise up 2cm
yvec_des = zvec_des.cross(xvec_des); // construct consistent right-hand triad
Rmat.col(0)= xvec_des;
Rmat.col(1)= yvec_des;
Rmat.col(2)= zvec_des;
Affine_des_gripper.linear() = Rmat;
Affine_des_gripper.translation() = origin_des;
ROS_INFO_STREAM("des origin: " << Affine_des_gripper.translation().transpose() << endl);
ROS_INFO_STREAM("orientation: " << endl);
ROS_INFO_STREAM(Rmat << endl);
// convert des pose from Eigen::Affine to geometry_msgs::PoseStamped
rt_tool_pose.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
// send move plan request
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
// send command to execute planned motion
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
/* New code added to enable wiping motion */
geometry_msgs::PoseStamped rt_tool_wipe_table;
origin_des[1] += 0.1;
Affine_des_gripper.translation() = origin_des;
rt_tool_wipe_table.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_wipe_table);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
} else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
origin_des[1] -= 0.2;
Affine_des_gripper.translation() = origin_des;
rt_tool_wipe_table.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_wipe_table);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS){
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
} else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
origin_des[1] += 0.2;
Affine_des_gripper.translation() = origin_des;
rt_tool_wipe_table.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_wipe_table);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS){
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
} else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
origin_des[1] -= 0.1;
Affine_des_gripper.translation() = origin_des;
rt_tool_wipe_table.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_wipe_table);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS){
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
} else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
/* end of new code */
} else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
}
ros::Duration(0.5).sleep(); // sleep for half a second
ros::spinOnce();
}
return 0;
}
int main(int argc, char** argv) {
ros::init(argc, argv, "ps9_main");
ros::NodeHandle nh;
// instantiate block detection object
Block_detection block_detection(&nh);
// instantiate an yale hand gripper object
Gripper baxter_gripper_control(&nh);
// instantiate an arm motion object
ArmMotionCommander arm_motion_commander(&nh);
// instantiate a human hand detection object
HumanMachineInterface human_machine_interface(&nh);
// preparation work for yale gripper
// open hand for usage
baxter_gripper_control.open_hand_w_position();
// VARIABLES TO BE USED IN THE LOOP
// for point cloud sensor
geometry_msgs::Pose block_pose; // pose of the block
int block_color; // 1: red, 2: green, 3: blue
double block_orientation; // block orientation in torso frame
ROS_INFO_STREAM("block position: " <<
block_pose.position.x << ", " <<
block_pose.position.y << ", " <<
block_pose.position.z);
ROS_INFO_STREAM("block orientation(w): " << block_pose.orientation.w);
// for gripper control
// none
// for arm motion commander
Eigen::Affine3d Affine_des_gripper;
Eigen::Vector3d xvec_des,yvec_des,zvec_des,origin_des;
Eigen::Vector3f major_axis, centroid;
zvec_des << 0,0,-1; // pointing to the ground
Eigen::Matrix3d Rmat;
int rtn_val;
double plane_dist;
geometry_msgs::PoseStamped rt_tool_pose_origin; // right hand pose in the selected points
geometry_msgs::PoseStamped rt_tool_pose_upper; // right hand pose upper from origin
geometry_msgs::PoseStamped rt_tool_pose_red_des; // right hand destination pose for red block
geometry_msgs::PoseStamped rt_tool_pose_green_des; // right hand destination pose for green block
geometry_msgs::PoseStamped rt_tool_pose_blue_des; // right hand destination pose for blue block
// prepare the destination position for different colors
// orientation
xvec_des << 1,0,0; // major direction pointing to the front
yvec_des = zvec_des.cross(xvec_des);
Rmat.col(0) = xvec_des;
Rmat.col(1) = yvec_des;
Rmat.col(2) = zvec_des;
Affine_des_gripper.linear() = Rmat;
// position, change the position for different colors here
// for red
origin_des[0] = 0.5;
origin_des[1] = 0.3;
origin_des[2] = 0.2;
Affine_des_gripper.translation() = origin_des;
rt_tool_pose_red_des.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
// for green
origin_des[0] = 0.45;
origin_des[1] = 0.0;
origin_des[2] = 0.2;
Affine_des_gripper.translation() = origin_des;
rt_tool_pose_green_des.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
// for blue
origin_des[0] = 0.5;
origin_des[1] = -0.3;
origin_des[2] = 0.2;
Affine_des_gripper.translation() = origin_des;
rt_tool_pose_blue_des.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
// pre movement, move to pre-define pose in joint space
rtn_val = arm_motion_commander.plan_move_to_pre_pose(); // plan the path
rtn_val = arm_motion_commander.rt_arm_execute_planned_path(); // execute the planned path
// for hand detection in human machine interface
bool b_human_hand_present; // bool value to indicate current human hand status
bool b_continue_blocks_operation; // bool value to indicate whether continue operation on blocks
// THE LOOP
while (ros::ok()) {
// baxter should be able to recognize the following human hand signals in sequence
// human hand signal 1: present
// human hand signal 2: not present
// these signals indicate a permit of performing block detection and baxter movements
b_human_hand_present = human_machine_interface.get_human_hand();
while (!b_human_hand_present) {
// wait for 0.5 second and re-check
ros::Duration(0.5).sleep();
ROS_INFO("***** msg from HMI *****");
b_human_hand_present = human_machine_interface.get_human_hand();
ROS_INFO("waiting for human hand");
}
// if here, get an human hand presence signal
ROS_INFO("human hand signal 1 detected");
int time_count = 0; // time count for hand presence
while (b_human_hand_present && time_count<10) {
ros::Duration(1.0).sleep();
ROS_INFO("***** msg from HMI *****");
b_human_hand_present = human_machine_interface.get_human_hand();
ROS_INFO("waiting for hand to be removed");
time_count = time_count + 1; // usually an human interaction is within 10 seconds
// this will avoid program accidently waits here forever
}
if (!b_human_hand_present) { // the while-loop exits because hand signal is detected
b_continue_blocks_operation = true;
ROS_INFO("human hand signal 2 detected");
}
else { // the while-loop exits because time is out
b_continue_blocks_operation = false;
ROS_ERROR("time out on human hand signal 2");
}
// continue operation on blocks
if (b_continue_blocks_operation) {
ros::Duration(1.0).sleep(); // let people walk away
// begin trying to find the blocks
if (block_detection.find_stool()) {
// stool is within range
block_color = 0;
block_color = block_detection.find_block(); // get the color of the block
ROS_INFO_STREAM("block color code: " << block_color);
// if 0, no block is found
if (block_color) {
ROS_INFO("stool is found, block is found");
ROS_INFO_STREAM("color of the block: " << block_color << " (1-red, 2-green, 3-blue)");
block_pose = block_detection.find_pose(); // calculate block pose
block_pose.position.z = block_pose.position.z + z_offset;
block_orientation = block_pose.orientation.w;
block_orientation = 2 * acos(block_orientation); // angle value
ROS_INFO_STREAM("block position: " <<
block_pose.position.x << ", " <<
block_pose.position.y << ", " <<
block_pose.position.z);
ROS_INFO_STREAM("block orientation(w): " << block_pose.orientation.w);
// ROS_INFO_STREAM("block orientation: " << block_orientation);
//setting rotations for gripper to be along block's major axis
xvec_des << cos(block_orientation), sin(block_orientation), 0;
yvec_des = zvec_des.cross(xvec_des);
Rmat.col(0) = xvec_des;
Rmat.col(1) = yvec_des;
Rmat.col(2) = zvec_des;// the z direction of the gripper
ROS_INFO_STREAM("xvec_des: " << xvec_des[0] << ", " << xvec_des[1] << ", " << xvec_des[2]);
ROS_INFO_STREAM("yvec_des: " << yvec_des[0] << ", " << yvec_des[1] << ", " << yvec_des[2]);
ROS_INFO_STREAM("zvec_des: " << zvec_des[0] << ", " << zvec_des[1] << ", " << zvec_des[2]);
Affine_des_gripper.linear() = Rmat;
// the position
origin_des[0] = block_pose.position.x;
origin_des[1] = block_pose.position.y;
origin_des[2] = block_pose.position.z - 0.01; // block height is 0.03
Affine_des_gripper.translation() = origin_des;
// for rt_tool_pose_origin
rt_tool_pose_origin.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
// for rt_tool_pose_upper
origin_des[2] = block_pose.position.z + 0.1; // the upper area of block
Affine_des_gripper.translation() = origin_des;
rt_tool_pose_upper.pose = arm_motion_commander.transformEigenAffine3dToPose(Affine_des_gripper);
// 1.move the gripper to the upper area of the block
ROS_INFO_STREAM(""); // blank line here
ROS_INFO("move 1: move the gripper to the upper area of the block");
ROS_INFO_STREAM("des pos: " << rt_tool_pose_upper.pose.position.x
<< ", " << rt_tool_pose_upper.pose.position.y
<< ", " << rt_tool_pose_upper.pose.position.z);
// send move plan request
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose_upper);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
// send command to execute planned motion
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
}
else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
ros::Duration(0.25).sleep();
// 2.move the gripper to the origin of the block
ROS_INFO_STREAM(""); // blank line here
ROS_INFO("move 2: move the gripper to the origin of the block");
ROS_INFO_STREAM("des pos: " << rt_tool_pose_origin.pose.position.x
<< ", " << rt_tool_pose_origin.pose.position.y
<< ", " << rt_tool_pose_origin.pose.position.z);
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose_origin);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
// send command to execute planned motion
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
}
else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
// 3.grasp the block with the gripper
ROS_INFO_STREAM(""); // blank line here
ROS_INFO("move 3: grasp the block with the gripper");
baxter_gripper_control.close_hand_w_torque();
ros::Duration(0.25).sleep();
// 4.move the gripper to the upper area of the block
ROS_INFO_STREAM(""); // blank line here
ROS_INFO("move 4: move the gripper to the upper area of the block");
ROS_INFO_STREAM("des pos: " << rt_tool_pose_upper.pose.position.x
<< ", " << rt_tool_pose_upper.pose.position.y
<< ", " << rt_tool_pose_upper.pose.position.z);
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose_upper);
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
// send command to execute planned motion
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
}
else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
ros::Duration(0.25).sleep();
// 5.move the gripper to destination according to block color
ROS_INFO_STREAM(""); // blank line here
ROS_INFO("move 5: move the gripper to destination according to block color");
// set planed motion according to block color
switch (block_color) {
case 1: // color is red
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose_red_des);
ROS_INFO_STREAM("des pos: " << rt_tool_pose_red_des.pose.position.x
<< ", " << rt_tool_pose_red_des.pose.position.y
<< ", " << rt_tool_pose_red_des.pose.position.z);
break;
case 2: // color is green
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose_green_des);
ROS_INFO_STREAM("des pos: " << rt_tool_pose_green_des.pose.position.x
<< ", " << rt_tool_pose_green_des.pose.position.y
<< ", " << rt_tool_pose_green_des.pose.position.z);
break;
case 3: // color is blue
rtn_val = arm_motion_commander.rt_arm_plan_path_current_to_goal_pose(rt_tool_pose_blue_des);
ROS_INFO_STREAM("des pos: " << rt_tool_pose_blue_des.pose.position.x
<< ", " << rt_tool_pose_blue_des.pose.position.y
<< ", " << rt_tool_pose_blue_des.pose.position.z);
break;
}
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
// send command to execute planned motion
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
}
else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
// 6.release the block
ROS_INFO_STREAM(""); // blank line here
ROS_INFO("move 6: release the block");
baxter_gripper_control.open_hand_w_position();
// 7.move the gripper to pre-pose
ROS_INFO_STREAM(""); // blank line here
ROS_INFO("move 7: move the gripper to pre-pose");
rtn_val = arm_motion_commander.plan_move_to_pre_pose();
if (rtn_val == cwru_action::cwru_baxter_cart_moveResult::SUCCESS) {
// send command to execute planned motion
rtn_val = arm_motion_commander.rt_arm_execute_planned_path();
}
else {
ROS_WARN("Cartesian path to desired pose not achievable");
}
}
else {
ROS_WARN("stool is found, block is not found");
}
}
else {
ROS_WARN("stool is not found");
ros::Duration(1.0).sleep(); // sleep 1 second each time
}
}
ros::spinOnce(); // let variables to update, if there is any
}
return 0;
}
