void demoPlace(move_group_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::PlaceLocation> loc;
for (std::size_t i = 0 ; i < 20 ; ++i)
{
geometry_msgs::PoseStamped p = group.getRandomPose();
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::PlaceLocation g;
g.place_pose = p;
g.approach.direction.vector.x = 1.0;
g.retreat.direction.vector.z = 1.0;
g.retreat.direction.header = p.header;
g.approach.min_distance = 0.2;
g.approach.desired_distance = 0.4;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.27;
g.post_place_posture.name.resize(1, "r_gripper_joint");
g.post_place_posture.position.resize(1);
g.post_place_posture.position[0] = 0;
loc.push_back(g);
}
group.place("bubu", loc);
}
void place(move_group_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::PlaceLocation> loc;
geometry_msgs::PoseStamped p;
p.header.frame_id = "base_footprint";
p.pose.position.x = 0.42;
p.pose.position.y = 0.0;
p.pose.position.z = 0.5;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::PlaceLocation g;
g.place_pose = p;
g.approach.direction.vector.z = -1.0;
g.retreat.direction.vector.x = -1.0;
g.retreat.direction.header.frame_id = "base_footprint";
g.approach.direction.header.frame_id = "r_wrist_roll_link";
g.approach.min_distance = 0.1;
g.approach.desired_distance = 0.2;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.25;
g.post_place_posture.name.resize(1, "r_gripper_joint");
g.post_place_posture.position.resize(1);
g.post_place_posture.position[0] = 1;
loc.push_back(g);
group.setSupportSurfaceName("table");
// add path constraints
moveit_msgs::Constraints constr;
constr.orientation_constraints.resize(1);
moveit_msgs::OrientationConstraint &ocm = constr.orientation_constraints[0];
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = p.header.frame_id;
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.2;
ocm.absolute_z_axis_tolerance = M_PI;
ocm.weight = 1.0;
group.setPathConstraints(constr);
group.setPlannerId("RRTConnectkConfigDefault");
group.place("part", loc);
}
void set_joint_goal() {
group->setJointValueTarget("katana_motor1_pan_joint", -1.51);
group->setJointValueTarget("katana_motor2_lift_joint",
2.13549384276445);
group->setJointValueTarget("katana_motor3_lift_joint",
-2.1556486321117725);
group->setJointValueTarget("katana_motor4_lift_joint",
-1.971949347057968);
group->setJointValueTarget("katana_motor5_wrist_roll_joint", 0.0);
return;
}
void demoFollowConstraints(move_group_interface::MoveGroup &group)
{
geometry_msgs::PoseStamped curr = group.getCurrentPose();
std::vector<geometry_msgs::Pose> c(2);
c[0] = curr.pose;
c[1] = c[0];
c[1].position.x -= 0.01;
group.followConstraints(c);
move_group_interface::MoveGroup::Plan p;
group.plan(p);
}
void move(ros::Publisher pub, move_group_interface::MoveGroup group, std::vector<double> joints_target, std::vector<std::string> joint_names, double speed, double thres_in)
{
std::vector<double> joints_curr;
std::vector<double> joint_error;
std::vector<double> joint_vel_com;
double error_magnitude;
double alpha = 1; // step size
double joint_vel_com_mag;
double max_vel_mag = 0.08;
if (speed < max_vel_mag)
max_vel_mag = speed;
double thres = 0.01;
if (thres_in < thres)
thres = thres_in;
for (int i=0; i< 100000; i++)
{
//~ std::cout << "hi 1" << std::endl;
joints_curr = group.getCurrentJointValues();
joint_error = joints_curr;
joint_vel_com = joints_curr;
//~ std::cout << "hi 2" << std::endl;
// calculate joint error, error magnitude, comm vel
error_magnitude = 0;
for (int j=0; j< joint_error.size(); j++) {
joint_error[j] = joints_target[j] - joints_curr[j];
joint_vel_com[j] = joint_error[j] * alpha;
error_magnitude += joint_error[j]*joint_error[j];
}
//~ std::cout << "hi 3" << std::endl;
error_magnitude = sqrt(error_magnitude);
joint_vel_com_mag = error_magnitude*alpha;
//~ std::cout << "hi 4" << std::endl;
// if comm vel exceeds max, scale it back to max
if (joint_vel_com_mag > max_vel_mag)
for (int j=0; j< joint_vel_com.size(); j++)
joint_vel_com[j] = max_vel_mag * joint_vel_com[j] / joint_vel_com_mag;
//~ std::cout << "hi 5" << std::endl;
setJointValues(pub, baxter_core_msgs::JointCommand::VELOCITY_MODE, joint_vel_com, joint_names);
std::cout << "iter: " << i << " error_magnitude: " << error_magnitude << std::endl;
//~ std::cout << "hi 6" << std::endl;
if (error_magnitude < thres)
break;
//~ std::cout << "hi 7" << std::endl;
usleep(1000);
}
// need to reset to position mode when we're done or the robot will disable itself...
setJointValues(pub, baxter_core_msgs::JointCommand::POSITION_MODE, joints_target, joint_names);
}
int move_arm_out_of_the_way() {
//set_joint_goal();
//clear_collision_map();
ROS_INFO("Move arm out of the way.");
//move arm to initial home state as defined in the urdf
group->setNamedTarget("home_stable");
group->asyncMove();
//clear_collision_map();
ROS_INFO("Arm moved out of the way.");
return 1;
}
void pick(move_group_interface::MoveGroup &group)
{
std::vector<manipulation_msgs::Grasp> grasps;
geometry_msgs::PoseStamped p;
p.header.frame_id = "base_footprint";
p.pose.position.x = 0.32;
p.pose.position.y = -0.7;
p.pose.position.z = 0.5;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
manipulation_msgs::Grasp g;
g.grasp_pose = p;
g.approach.direction.vector.x = 1.0;
g.approach.direction.header.frame_id = "r_wrist_roll_link";
g.approach.min_distance = 0.2;
g.approach.desired_distance = 0.4;
g.retreat.direction.header.frame_id = "base_footprint";
g.retreat.direction.vector.z = 1.0;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.25;
g.pre_grasp_posture.name.resize(1, "r_gripper_joint");
g.pre_grasp_posture.position.resize(1);
g.pre_grasp_posture.position[0] = 1;
g.grasp_posture.name.resize(1, "r_gripper_joint");
g.grasp_posture.position.resize(1);
g.grasp_posture.position[0] = 0;
grasps.push_back(g);
group.setSupportSurfaceName("table");
group.pick("part", grasps);
}
void move_to_wait_position(move_group_interface::MoveGroup& move_group)
{
//ROS_ERROR_STREAM("move_to_wait_position is not implemented yet. Aborting."); exit(1);
// task variables
bool success; // saves the move result
// set robot wait target
/* Fill Code: [ use the 'setNamedTarget' method in the 'move_group' object] */
move_group.setNamedTarget(cfg.WAIT_POSE_NAME);
// move the robot
/* Fill Code: [ use the 'move' method in the 'move_group' object and save the result in the 'success' variable] */
success = move_group.move();
if(success)
{
ROS_INFO_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Succeeded");
}
else
{
ROS_ERROR_STREAM("Move " << cfg.WAIT_POSE_NAME<< " Failed");
exit(1);
}
}
bool place() {
//group->place(object_to_manipulate, generated_pickup_grasp.grasp_pose);
std::vector<manipulation_msgs::PlaceLocation> loc;
geometry_msgs::PoseStamped p;
/*
p.header.frame_id = BASE_LINK;
p.pose.position = object_to_manipulate_position.point;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
*/
p = generated_pickup_grasp.grasp_pose;
make_pose_reachable_by_5DOF_katana(p);
ROS_DEBUG_STREAM("Place Pose: " << p.pose);
manipulation_msgs::PlaceLocation g;
g.place_pose = p;
g.approach.direction.vector.z = -1.0;
g.retreat.direction.vector.x = -1.0;
g.retreat.direction.header.frame_id = BASE_LINK;
g.approach.direction.header.frame_id = GRIPPER_FRAME;
g.approach.min_distance = 0.1;
g.approach.desired_distance = 0.2;
g.retreat.min_distance = 0.1;
g.retreat.desired_distance = 0.25;
g.post_place_posture.name.resize(1, FINGER_JOINT);
g.post_place_posture.position.resize(1);
g.post_place_posture.position[0] = 0.30;
loc.push_back(g);
//group->setSupportSurfaceName("table");
/* Option path constraints (e.g. to always stay upright)
// add path constraints
moveit_msgs::Constraints constr;
constr.orientation_constraints.resize(1);
moveit_msgs::OrientationConstraint &ocm = constr.orientation_constraints[0];
ocm.link_name = "r_wrist_roll_link";
ocm.header.frame_id = p.header.frame_id;
ocm.orientation.x = 0.0;
ocm.orientation.y = 0.0;
ocm.orientation.z = 0.0;
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.2;
ocm.absolute_y_axis_tolerance = 0.2;
ocm.absolute_z_axis_tolerance = M_PI;
ocm.weight = 1.0;
group->setPathConstraints(constr);
group->setPlannerId("RRTConnectkConfigDefault");
*/
group->place(object_to_manipulate, loc);
/*
//create a place location
//geometry_msgs::PoseStamped place_location = pickup_location;
geometry_msgs::PoseStamped place_location;
place_location.header.stamp = ros::Time::now();
// ----- put the object down
ROS_INFO("Calling the place action");
object_manipulation_msgs::PlaceGoal place_goal;
place_location.header.frame_id = KATANA_BASE_LINK;
place_location.pose.position.x =
normalPoseRobotFrame.pose.position.x;
place_location.pose.position.y =
normalPoseRobotFrame.pose.position.y;
place_location.pose.position.z =
pickup_result.grasp.grasp_pose.position.z; //TODO: to calculate z object higth needs to be considered
tf::Transform position;
tf::poseMsgToTF(place_location.pose, position);
tf::Vector3 shift_direction;
shift_direction.setX(place_location.pose.position.x);
shift_direction.setY(place_location.pose.position.y);
shift_direction.setZ(0);
shift_direction.normalize();
tf::Vector3 shift_direction_inverse;
shift_direction_inverse.setX(-shift_direction.getX());
shift_direction_inverse.setY(-shift_direction.getY());
shift_direction_inverse.setZ(-shift_direction.getZ());
//transform place_position to the start of the test line
tf::Transform trans(tf::Quaternion(0, 0, 0, 1.0),
shift_direction_inverse * place_position_tolerance_in_meter);
position = trans * position;
//move along the test line and add positions to place_goal.place_locations
for (float offset = 0; offset <= place_position_tolerance_in_meter;
offset += place_planner_step_size_in_meter) {
tf::Transform trans(tf::Quaternion(0, 0, 0, 1.0),
shift_direction * place_planner_step_size_in_meter);
position = trans * position;
geometry_msgs::Pose position_pose;
tf::poseTFToMsg(position, position_pose);
place_location.pose = position_pose;
// Convert quaternion to RPY.
tf::Quaternion q;
double roll, pitch, yaw;
tf::quaternionMsgToTF(pickup_result.grasp.grasp_pose.orientation,
q);
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
//determine yaw which is compatible with the Katana 300 180 kinematics.
yaw = atan2(place_location.pose.position.y,
place_location.pose.position.x);
tf::Quaternion quat = tf::createQuaternionFromRPY(0.0, pitch, yaw);
place_location.pose.orientation.x = quat.getX();
place_location.pose.orientation.y = quat.getY();
place_location.pose.orientation.z = quat.getZ();
place_location.pose.orientation.w = quat.getW();
ROS_INFO(
"Place Location: XYZ, Quaternions xyzw = %lf,%lf,%lf, (%lf, %lf, %lf, %lf)", place_location.pose.position.x, place_location.pose.position.y, place_location.pose.position.z, place_location.pose.orientation.x, place_location.pose.orientation.y, place_location.pose.orientation.z, place_location.pose.orientation.w);
place_goal.place_locations.push_back(place_location);
}
//end place planner
//the collision names of both the objects and the table
//same as in the pickup action
place_goal.collision_object_name =
processing_call.response.collision_object_names.at(
object_to_pick_ind);
place_goal.collision_support_surface_name =
processing_call.response.collision_support_surface_name;
//set grasp orientation to identity (all info is already given by place_location)
place_goal.grasp.grasp_pose.orientation.x = 0;
place_goal.grasp.grasp_pose.orientation.y = 0;
place_goal.grasp.grasp_pose.orientation.z = 0;
place_goal.grasp.grasp_pose.orientation.w = 1;
place_goal.grasp.grasp_pose.position.x = 0;
place_goal.grasp.grasp_pose.position.y = 0;
place_goal.grasp.grasp_pose.position.z = 0;
//use the arm to place
place_goal.arm_name = "arm";
//padding used when determining if the requested place location
//would bring the object in collision with the environment
place_goal.place_padding = 0.02;
//how much the gripper should retreat after placing the object
place_goal.desired_retreat_distance = 0.06;
place_goal.min_retreat_distance = 0.05;
//we will be putting down the object along the "vertical" direction
//which is along the z axis in the base_link frame
geometry_msgs::Vector3Stamped direction;
direction.header.stamp = ros::Time::now();
direction.header.frame_id = KATANA_BASE_LINK;
direction.vector.x = 0;
direction.vector.y = 0;
direction.vector.z = -1;
place_goal.approach.direction = direction;
//request a vertical put down motion of 10cm before placing the object
place_goal.approach.desired_distance = 0.06;
place_goal.approach.min_distance = 0.05;
//we are not using tactile based placing
place_goal.use_reactive_place = false;
//send the goal
place_client->sendGoal(place_goal);
while (!place_client->waitForResult(ros::Duration(10.0))) {
ROS_INFO("Waiting for the place action...");
if (!nh.ok())
return -1;
}
object_manipulation_msgs::PlaceResult place_result =
*(place_client->getResult());
if (place_client->getState()
!= actionlib::SimpleClientGoalState::SUCCEEDED) {
if (place_result.manipulation_result.value
== object_manipulation_msgs::ManipulationResult::RETREAT_FAILED) {
ROS_WARN(
"Place failed with error RETREAT_FAILED, ignoring! This may lead to collision with the object we just placed!");
} else {
ROS_ERROR(
"Place failed with error code %d", place_result.manipulation_result.value);
return -1;
}
}
//success!
ROS_INFO("Success! Object moved.");
move_arm_out_of_the_way();
object_in_gripper = 0;
*/
return true;
}
int pickup() {
/*
// ----- pick up object near point (in meter) relative to base_footprint
geometry_msgs::PointStamped pickup_point;
pickup_point.header.frame_id = KURTANA_BASE_LINK;
pickup_point.point.x = desired_pickup_point.x;
pickup_point.point.y = desired_pickup_point.y;
pickup_point.point.z = desired_pickup_point.z;
*/
// ----- call the tabletop detection
ROS_INFO("Calling tabletop detector");
tabletop_object_detector::TabletopDetection detection_call;
//we want recognized database objects returned
//set this to false if you are using the pipeline without the database
detection_call.request.return_models = false;
//we want the individual object point clouds returned as well
detection_call.request.return_clusters = false;
if (!object_detection_srv.call(detection_call)) {
ROS_ERROR("Tabletop detection service failed");
return -1;
}
if (detection_call.response.detection.result
!= detection_call.response.detection.SUCCESS) {
ROS_ERROR(
"Tabletop detection returned error code %d", detection_call.response.detection.result);
return -1;
}
if (detection_call.response.detection.clusters.empty()
&& detection_call.response.detection.models.empty()) {
ROS_ERROR(
"The tabletop detector detected the table, but found no objects");
return -1;
}
//Remove the table because there are convex hull problems if adding the table to envirnonment
//detection_call.response.detection.table.convex_hull = shape_msgs::Mesh();
tabletop_collision_map_processing::TabletopCollisionMapProcessing process_call;
process_call.request.detection_result = detection_call.response.detection;
process_call.request.reset_collision_models = false;
process_call.request.reset_attached_models = false;
if (!collision_processing_srv.call(process_call)) {
ROS_ERROR("Tabletop Collision Map Processing failed");
return -1;
}
ROS_INFO_STREAM("Found objects count: " << process_call.response.collision_object_names.size());
if (process_call.response.collision_object_names.empty()) {
ROS_ERROR("Tabletop Collision Map Processing error");
return -1;
}
/*
ROS_INFO("Add table to planning scene");
moveit_msgs::CollisionObject collision_object;
collision_object.header.stamp = ros::Time::now();
collision_object.header.frame_id =
detection_call.response.detection.table.pose.header.frame_id;
ROS_INFO_STREAM("Add clusters");
object_positions.clear();
//add objects to planning scene
for (unsigned int i = 0;
i < detection_call.response.detection.clusters.size(); i++) {
sensor_msgs::PointCloud2 pc2;
sensor_msgs::convertPointCloudToPointCloud2(
detection_call.response.detection.clusters[i], pc2);
geometry_msgs::PoseStamped poseStamped;
geometry_msgs::Vector3 dimension;
getClusterBoundingBox3D(pc2, poseStamped, dimension);
geometry_msgs::PointStamped point;
point.header.frame_id = poseStamped.header.frame_id;
point.point = poseStamped.pose.position;
object_positions.push_back(point);
ostringstream id;
id << "object " << i;
collision_object.id = id.str().c_str();
ROS_INFO_STREAM("Object id: " << collision_object.id);
collision_object.operation = moveit_msgs::CollisionObject::REMOVE;
pub_collision_object.publish(collision_object);
collision_object.operation = moveit_msgs::CollisionObject::ADD;
collision_object.primitives.resize(1);
collision_object.primitives[0].type =
shape_msgs::SolidPrimitive::BOX;
collision_object.primitives[0].dimensions.resize(
shape_tools::SolidPrimitiveDimCount<
shape_msgs::SolidPrimitive::BOX>::value);
collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_X] =
dimension.x;
collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Y] =
dimension.y;
collision_object.primitives[0].dimensions[shape_msgs::SolidPrimitive::BOX_Z] =
dimension.z;
collision_object.primitive_poses.resize(1);
collision_object.primitive_poses[0] = poseStamped.pose;
pub_collision_object.publish(collision_object);
}
*/
//group->setSupportSurfaceName("table");
//call object pickup
ROS_INFO("Calling the pickup action");
generated_pickup_grasp = generateGrasp();
ROS_INFO("Get nearest object");
object_to_manipulate = nearest_object();
//test
/*
[DEBUG] [1378456372.242171284]: IK pose: position:
x: 0.44866
y: -0.185202
z: 0.392208
orientation:
x: 0.00125435
y: -0.00632681
z: -0.194471
w: 0.980887
[DEBUG] [1378456372.242351955]: Found 4 solutions from IKFast
[DEBUG] [1378456372.242443678]: Sol 0: -3.914825e-01 1.548975e+00 -1.572923e+00 -3.587233e-02 -4.921995e-03 2.618548e-322
[DEBUG] [1378456372.243036938]: Solution passes callback
geometry_msgs::PoseStamped p;
p.header.frame_id = ARM_BASE_LINK;
p.pose.position.x = 0.44866;
p.pose.position.y = -0.185202;
p.pose.position.z = 0.392208;
p.pose.orientation.x = 0;
p.pose.orientation.y = 0;
p.pose.orientation.z = 0;
p.pose.orientation.w = 1;
group->setPoseTarget(p,GRIPPER_FRAME);
group->move();
*/
//group->pick(object_to_manipulate);
ROS_INFO_STREAM("Picking up Object: " << object_to_manipulate);
group->pick(object_to_manipulate, generated_pickup_grasp);
ROS_INFO("Pick returned!!!!!11111 OMGWTFIT");
//group->place(object_to_manipulate);
std::vector<double> rpy = group->getCurrentRPY(
group->getEndEffectorLink());
ROS_INFO_STREAM(
"End effector link: " << rpy.at(0)<< rpy.at(1) << rpy.at(2));
object_in_gripper = 1;
move_arm_out_of_the_way();
return 0;
}
Pick_and_place_app(ros::NodeHandle *_nh) {
nh = *_nh;
//get parameters from parameter server
nh.param<int>("sim", sim, 0);
nh.param<int>("DIRECTLY_SELECT_GRASP_POSE", DIRECTLY_SELECT_GRASP_POSE,
1);
//the distance between the surface of the object to grasp and the GRIPPER_FRAME origin
nh.param<double>("OBJECT_GRIPPER_STANDOFF", STANDOFF, 0.08);
nh.param<std::string>("ARM_BASE_LINK", ARM_BASE_LINK,
"katana_base_link");
nh.param<std::string>("BASE_LINK", BASE_LINK, "base_link");
nh.param<std::string>("GRIPPER_FRAME", GRIPPER_FRAME,
"katana_gripper_tool_frame");
nh.param<std::string>("FINGER_JOINT", FINGER_JOINT,
"katana_l_finger_joint");
nh.param<std::string>("ARM_NAME", ARM_NAME, "arm");
clicked = 0;
object_in_gripper = 0;
pcl_input_point_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>());
//pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
// create TF listener
tf_listener = new tf::TransformListener();
pub_collision_object = nh.advertise<moveit_msgs::CollisionObject>(
"collision_object", 10);
ros::WallDuration(1.0).sleep();
group = new move_group_interface::MoveGroup(ARM_NAME);
//group->setPoseReferenceFrame(BASE_LINK);
//group->setPlanningTime(5.0);
group->setGoalTolerance(0.01);
group->setGoalOrientationTolerance(0.005);
group->allowReplanning(true);
group->setWorkspace(-0.5,-0.6,-0.3,0.6,0.6,1.5);
//wait for get planning scene server
while(!ros::service::waitForService (GET_PLANNING_SCENE_SERVICE_NAME, ros::Duration(2.0)) && nh.ok()){
ROS_INFO("Waiting for get planning scene service to come up");
}
if (!nh.ok())
exit(0);
get_planning_scene_srv = nh.serviceClient<moveit_msgs::GetPlanningScene>(GET_PLANNING_SCENE_SERVICE_NAME, true);
//wait for detection client
while (!ros::service::waitForService(OBJECT_DETECTION_SERVICE_NAME,
ros::Duration(2.0)) && nh.ok()) {
ROS_INFO("Waiting for object detection service to come up");
}
if (!nh.ok())
exit(0);
object_detection_srv = nh.serviceClient<
tabletop_object_detector::TabletopDetection>(
OBJECT_DETECTION_SERVICE_NAME, true);
//wait for collision map processing client
while (!ros::service::waitForService(COLLISION_PROCESSING_SERVICE_NAME,
ros::Duration(2.0)) && nh.ok()) {
ROS_INFO("Waiting for collision processing service to come up");
}
if (!nh.ok())
exit(0);
collision_processing_srv =
nh.serviceClient<
tabletop_collision_map_processing::TabletopCollisionMapProcessing>(
COLLISION_PROCESSING_SERVICE_NAME, true);
cluster_bounding_box2_3d_client_ = nh.serviceClient<
object_manipulation_msgs::FindClusterBoundingBox2>(
CLUSTER_BOUNDING_BOX2_3D_NAME, true);
vis_marker_publisher = nh.advertise<visualization_msgs::Marker>(
"pick_and_place_markers", 128);
//Sets the kinects tilt angle
set_kinect_ptu("kurtana_pitch_joint", 0.75);
ROS_INFO("Kinect lined up.");
move_arm_out_of_the_way();
}