//! Points the high-def camera frame at a point in a given frame
void lookAt(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 high-def camera frame
//(pointing_axis defaults to X-axis)
goal.pointing_frame = "high_def_frame";
//take at least 0.5 seconds to get there
goal.min_duration = ros::Duration(0.5);
//and go no faster than 1 rad/s
goal.max_velocity = 1.0;
//send the goal
point_head_client_->sendGoal(goal);
//wait for it to get there (abort after 2 secs to prevent getting stuck)
point_head_client_->waitForResult(ros::Duration(2));
}
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());
}
}
bool pointHead (race_msgs::BoundingBox bb, float z=1.0)
{
bool success = true;
//the goal message we will be sending
pr2_controllers_msgs::PointHeadGoal goal;
//the point to be looking at is expressed in the "base_link" frame
geometry_msgs::PointStamped point;
//point.header.frame_id = bb.pose_stamped.header.frame_id;
//point.point.x = bb.pose_stamped.pose.position.x;
//point.point.y = bb.pose_stamped.pose.position.y;
point.header.frame_id = "/base_link";
point.point.x = 1.0;
point.point.y = 0.0;
point.point.z = z;
goal.target = point;
//we want the X axis of the camera frame to be pointing at the target
goal.pointing_frame = "high_def_frame";
goal.pointing_axis.x = 1;
goal.pointing_axis.y = 0;
goal.pointing_axis.z = 0;
//take at least 0.5 seconds to get there
goal.min_duration = ros::Duration(0.5);
//and go no faster than 1 rad/s
goal.max_velocity = 1.0;
//send the goal
point_head_client_->sendGoal(goal);
//wait for it to get there (abort after 2 secs to prevent getting stuck)
point_head_client_->waitForResult(ros::Duration(2));
return success;
}
