void move_head (std::string frame_id, double x, double y, double z)
{
//the goal message we will be sending
pr2_controllers_msgs::PointHeadGoal goal;
//the target point, expressed in the requested frame
geometry_msgs::PointStamped point;
point.header.frame_id = frame_id;
point.point.x = x; point.point.y = y; point.point.z = z;
goal.target = point;
//we are pointing the wide_stereo camera frame
//(pointing_axis defaults to X-axis)
goal.pointing_frame = "narrow_stereo_optical_frame";
//take at least 2 seconds to get there
goal.min_duration = ros::Duration(1);
//and go no faster than 0.1 rad/s
goal.max_velocity = 0.5;
//send the goal
point_head_client_->sendGoal(goal);
//wait for it to get there
bool finished_within_time = point_head_client_->waitForResult(ros::Duration(20.0));
if (!finished_within_time)
{
ROS_ERROR("Head did not move to pose: %f %f %f", point.point.x, point.point.y, point.point.z);
}
else
{
actionlib::SimpleClientGoalState state = point_head_client_->getState();
bool success = (state == actionlib::SimpleClientGoalState::SUCCEEDED);
if(success)
ROS_INFO("Action move head finished: %s position: %f %f %f", state.toString().c_str(), point.point.x, point.point.y, point.point.z);
else
ROS_ERROR("Action move head failed: %s", state.toString().c_str());
}
}
