Go to the documentation of this file.00001 #include <cmath>
00002 #include <ros/ros.h>
00003 #include <tf/transform_listener.h>
00004 #include <actionlib/client/simple_action_client.h>
00005 #include <pr2_controllers_msgs/PointHeadAction.h>
00006 #include <std_srvs/Empty.h>
00007
00008
00009 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;
00010
00011 class RobotHead
00012 {
00013 private:
00014 PointHeadClient* point_head_client_;
00015 std::string frame_to_point;
00016 std::string frame_to_track;
00017 tf::TransformListener listener;
00018 bool active_;
00019
00020 public:
00022 RobotHead() :frame_to_point("/wide_stereo_link"), frame_to_track("/checkerboard_0"), active_(false)
00023 {
00024
00025 point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
00026
00027
00028 while(!point_head_client_->waitForServer(ros::Duration(5.0))){
00029 ROS_INFO("Waiting for the point_head_action server to come up");
00030 }
00031 }
00032
00033 ~RobotHead()
00034 {
00035 delete point_head_client_;
00036 }
00037
00038 void lookAt(geometry_msgs::PoseStamped pose_stamped)
00039 {
00040
00041 pr2_controllers_msgs::PointHeadGoal goal;
00042 geometry_msgs::PointStamped point_stamped;
00043 point_stamped.header = pose_stamped.header;
00044 point_stamped.point = pose_stamped.pose.position;
00045 goal.target = point_stamped;
00046 goal.pointing_frame = this->frame_to_point;
00047
00048
00049 goal.min_duration = ros::Duration(1.5);
00050
00051
00052 goal.max_velocity = 1.25;
00053
00054
00055 point_head_client_->sendGoal(goal);
00056 point_head_client_->waitForResult(ros::Duration(5));
00057 }
00058
00060 void lookAt(std::string frame_id, double x, double y, double z, double max_wait_seconds = 10.0)
00061 {
00062
00063 pr2_controllers_msgs::PointHeadGoal goal;
00064
00065
00066 geometry_msgs::PointStamped point;
00067 point.header.frame_id = frame_id;
00068 point.point.x = x; point.point.y = y; point.point.z = z;
00069 goal.target = point;
00070
00071
00072
00073 goal.pointing_frame = this->frame_to_point;
00074
00075
00076 goal.min_duration = ros::Duration(0.5);
00077
00078
00079 goal.max_velocity = 0.25;
00080
00081
00082 point_head_client_->sendGoal(goal);
00083 ros::Time start = ros::Time::now();
00084 while(ros::ok() && !point_head_client_->waitForResult(ros::Duration(0.5))){
00085 if(start < (ros::Time::now() - ros::Duration(max_wait_seconds))){
00086 break;
00087 }
00088 ros::spinOnce();
00089 }
00090 }
00091
00092 void track(geometry_msgs::PoseStamped ps = geometry_msgs::PoseStamped() ){
00093
00094 if(active_) this->lookAt(ps);
00095 }
00096
00098 void searchAndSpin(float step)
00099 {
00100 srand(10);
00101 float radius = 2.0;
00102 float height = -0.40;
00103 while (ros::ok())
00104 {
00105 ros::spinOnce();
00106 ros::Duration(1.0).sleep();
00107 if(!active_)
00108 continue;
00109
00110 if(listener.waitForTransform(frame_to_track, frame_to_point, ros::Time::now()-ros::Duration(0.1), ros::Duration(4.0))){
00111 ROS_INFO("Transformable frame %s", frame_to_track.c_str());
00112 continue;
00113 }
00114 lookAt("/head_pan_link", radius, 0.0, height);
00115 if(listener.waitForTransform(frame_to_track, frame_to_point, ros::Time::now()-ros::Duration(0.1), ros::Duration(4.0)))
00116 continue;
00117
00118 lookAt("/torso_lift_link", radius*cos(step/180.0*M_PI),radius*sin(step/180.0*M_PI), height);
00119 if(listener.waitForTransform(frame_to_track, frame_to_point, ros::Time::now()-ros::Duration(0.1), ros::Duration(4.0)))
00120 continue;
00121
00122 step *= -1.0;
00123 height = (rand() % 100)/100.0 - 0.5;
00124 height *= height;
00125 radius = (rand() % 100)/100.0 + 1.0;
00126 ROS_INFO("Searching frame %s", frame_to_track.c_str());
00127 }
00128 }
00129
00130 bool switchActive(std_srvs::Empty::Request &req,
00131 std_srvs::Empty::Response &res )
00132 {
00133 active_ = true;
00134 lookAt("/torso_lift_link", 0.8, 0.3, 0.2);
00135
00136 return true;
00137 }
00138 bool switchInactive(std_srvs::Empty::Request &req,
00139 std_srvs::Empty::Response &res )
00140 {
00141 active_ = false;
00142 return true;
00143 }
00144 };
00145
00146
00147 int main(int argc, char** argv)
00148 {
00149
00150 ros::init(argc, argv, "look_around");
00151
00152 ros::NodeHandle n;
00153 RobotHead head;
00154 ros::Subscriber pose_sub = n.subscribe("pose0", 2, &RobotHead::track, &head);
00155 ros::ServiceServer service_on = n.advertiseService("search_and_track_checkerboard_on", &RobotHead::switchActive, &head);
00156 ros::ServiceServer service_off = n.advertiseService("search_and_track_checkerboard_off", &RobotHead::switchInactive, &head);
00157 ROS_INFO("(De-)Activation services advertised. Waiting for clients.");
00158 head.searchAndSpin(30);
00159 ros::spin();
00160 }
00161