new_look.cpp

Go to the documentation of this file.

#include <cmath>
#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <actionlib/client/simple_action_client.h>
#include <pr2_controllers_msgs/PointHeadAction.h>

// Our Action interface type, provided as a typedef for convenience
typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;

class RobotHead
{
private:
  PointHeadClient* point_head_client_;
  std::string frame_to_point;
  std::string frame_to_track;
  tf::TransformListener listener;
  ros::Time last_pose;
  float angle_range;
  float height_range;

public:
  RobotHead() :frame_to_point("/narrow_stereo_optical_frame"),
               frame_to_track("/checkerboard_0"),
               angle_range(45.0),
               height_range(0.2)
  {
    last_pose = ros::Time::now();
    //Initialize the client for the Action interface to the head controller
    point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);

    //wait for head controller action server to come up 
    while(!point_head_client_->waitForServer(ros::Duration(5.0))){
      ROS_INFO("Waiting for the point_head_action server to come up");
    }
  }

  ~RobotHead()
  {
    delete point_head_client_;
  }

  void lookAt(std::string frame_id, double x, double y, double z, double max_velocity = 0.15, double max_wait_seconds = 10.0)
  {
    ROS_INFO("Moving with velocity: %f", max_velocity);
    //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 = this->frame_to_point;

    //take at least 0.5 seconds to get there
    goal.min_duration = ros::Duration(0.4);

    //and go no faster than 1 rad/s
    goal.max_velocity = max_velocity;

    //send the goal
    point_head_client_->sendGoal(goal);
    ros::Time start = ros::Time::now();
    while(ros::ok() && !point_head_client_->waitForResult(ros::Duration(0.5))){
      if(start < (ros::Time::now() - ros::Duration(max_wait_seconds))){
        break;
      }
      ros::spinOnce();
    }
  }

  void track(geometry_msgs::PoseStamped ps){
    ROS_INFO("Adjusting look at checkerboard");
    last_pose = ros::Time::now();
    angle_range = 30.0;
    point_head_client_->cancelAllGoals();
    if(listener.waitForTransform(frame_to_track, frame_to_point, ros::Time::now(), ros::Duration(4.0)))
      lookAt(this->frame_to_track, 0,0,0, 0.01, 0.0);
    /*
    //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_to_track; // ps.header.frame_id;
    point.header.stamp = ros::Time::now();// ps.header.stamp;
    //point.point.x = ps.pose.position.x; point.point.y = ps.pose.position.y; point.point.z = ps.pose.position.z;
    point.point.x = 0.0;
    point.point.y = 0.0;
    point.point.z = 1.0;
    goal.target = point;

    //we are pointing the high-def camera frame 
    //(pointing_axis defaults to X-axis)
    goal.pointing_frame = ps.header.frame_id;

    //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 = 0.35;

    //send the goal
    point_head_client_->sendGoal(goal);

    ros::Time start = ros::Time::now();
    while(ros::ok() && !point_head_client_->waitForResult(ros::Duration(0.5))){
      if(start < (ros::Time::now() - ros::Duration(5.0))){
        break;
      }
    }
    */
  }

  void search_and_spin()
  {
    srand(10);
    float radius = 1.0;
    ros::Rate r(5); // HZ
    while (ros::ok())
    {
      ros::spinOnce();
      r.sleep();
      if(last_pose < (ros::Time::now() - ros::Duration(10.0))){
        ROS_INFO("Searching Checkerboard");
        float height = ((rand() % 100*height_range) - 50*height_range)/100.0;
        height *= height;
        //radius = (rand() % 100)/100.0 + 1.0; // in the range -0.7cm to 30 cm above 
        lookAt("/head_pan_link", radius, 0.0, height, 1.0); 
        if(last_pose > (ros::Time::now() - ros::Duration(10.0))){
          continue;
        }

        ROS_INFO("Searching...");
        angle_range*= -1.0;
        lookAt("/torso_lift_link", radius*cos(angle_range/180.0*M_PI),radius*sin(angle_range/180.0*M_PI), height+0.374, 1.0);//height difference betw torso lift link and head_pan_link
        if(abs(angle_range) < 120) angle_range *= 1.1;
        if(height_range < 1.0) angle_range *= 1.05;
      }
    }
  }
};

int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "checkerboard_tracker");
  ROS_INFO("Checkerboard Tracker started...");
  ros::NodeHandle n; 
  RobotHead head;
  ros::Subscriber pose_sub = n.subscribe("pose0", 5, &RobotHead::track, &head);
  head.search_and_spin();
}