search_and_track_marker.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>
#include <std_srvs/Empty.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;
  bool active_;
  bool found_;
  ros::Subscriber pose_sub_;
  ros::ServiceServer service_on_, service_off_;

public:
  RobotHead() : point_head_client_("/head_traj_controller/point_head_action", true),  //Initialize the client for the Action interface to the head controller
                frame_to_point("/head_mount_kinect_rgb_link"),
                frame_to_track("/checkerboard_0"),
                active_(false),
                found_(false)
  {
    ros::NodeHandle n; 
    pose_sub_ = n.subscribe("pose0", 2, &RobotHead::track, this);
    service_on_ = n.advertiseService("search_and_track_checkerboard_on", &RobotHead::switchActive, this);
    service_off_ = n.advertiseService("search_and_track_checkerboard_off", &RobotHead::switchInactive, this);

    ros::NodeHandle nh("~");
    nh.param<bool>("start_active", active_, active_); //By default wait for activation by service call

    //wait for head controller action server to come up 
    while(!point_head_client_.waitForServer(ros::Duration(1.0))){
      ROS_INFO_THROTTLE(10, "Waiting for the point_head_action server to come up");
    }

    ROS_INFO_COND(active_, "Searching for checkerboard. Deactivate by service call");
    ROS_INFO_COND(!active_, "Search for checkerboard available. Activate by service call");
  }

  void lookAt(geometry_msgs::PoseStamped pose_stamped)
  {
    if(!active_)return;
    found_ = true;
    //the goal message we will be sending
    pr2_controllers_msgs::PointHeadGoal goal;
    geometry_msgs::PointStamped point_stamped;
    point_stamped.header = pose_stamped.header;
    point_stamped.point = pose_stamped.pose.position;
    goal.target = point_stamped;
    goal.pointing_frame = this->frame_to_point;

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

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

    //send the goal
    point_head_client_.sendGoal(goal);
    point_head_client_.waitForResult(ros::Duration(5));
  }

  void lookAt(std::string frame_id, double x, double y, double z, double max_wait_seconds = 10.0)
  {
    if(!active_)return;
    //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.5);

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

    //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 = geometry_msgs::PoseStamped() ){
      //this->lookAt(this->frame_to_track, 0, 0, 0, 0.5);
      if(active_) this->lookAt(ps);
  }

  void searchAndSpin(float step)
  {
    srand(10);
    float radius = 2.0;
    float height = -0.40; 
    while (ros::ok())
    {
      ros::spinOnce();
      ros::Duration(1.0).sleep();
      if(!active_)
        continue;

      if(listener.waitForTransform(frame_to_track, frame_to_point, ros::Time::now()-ros::Duration(0.1), ros::Duration(4.0))){
        if(!found_){
          ROS_INFO("Transformable frame %s", frame_to_track.c_str());
          found_=true;
        } 
        continue;
      }
      else {
        found_ = false;
      }
      lookAt("/head_pan_link", radius, 0.0, height);
      if(listener.waitForTransform(frame_to_track, frame_to_point, ros::Time::now()-ros::Duration(0.1), ros::Duration(4.0))){
        if(!found_){
          ROS_INFO("Transformable frame %s", frame_to_track.c_str());
          found_=true;
        } 
        continue;
      }
      else {
        found_ = false;
      }

      lookAt("/torso_lift_link", radius*cos(step/180.0*M_PI),radius*sin(step/180.0*M_PI), height);
      if(listener.waitForTransform(frame_to_track, frame_to_point, ros::Time::now()-ros::Duration(0.1), ros::Duration(4.0))){
        if(!found_){
          ROS_INFO("Transformable frame %s", frame_to_track.c_str());
          found_=true;
        } 
        continue;
      }
      else {
        found_ = false;
      }
    
      step *= -1.0;
      height = (rand() % 100)/100.0 - 0.5; 
      height *= height;
      radius = (rand() % 100)/100.0 + 1.0; // in the range -0.7cm to 30 cm above 
      ROS_INFO("Searching frame %s", frame_to_track.c_str());
    }
  }

  bool switchActive(std_srvs::Empty::Request  &req,
                    std_srvs::Empty::Response &res )
  {
    active_ = true;
    lookAt("/torso_lift_link", 0.8, 0.3, 0.2);
    
    return true;
  }
  bool switchInactive(std_srvs::Empty::Request  &req,
                      std_srvs::Empty::Response &res )
  {
    active_ = false;
    return true;
  }
};


int main(int argc, char** argv)
{
  //init the ROS node
  ros::init(argc, argv, "look_around");
  
  RobotHead head;
  head.searchAndSpin(30);
  //ros::spin();
}