Head.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Thomas Ruehr <ruehr@cs.tum.edu>
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include <ros/ros.h>
00031 
00032 #include <actionlib/client/simple_action_client.h>
00033 #include <pr2_controllers_msgs/PointHeadAction.h>
00034 #include <ias_drawer_executive/Head.h>
00035 
00036 // Our Action interface type, provided as a typedef for convenience
00037 typedef actionlib::SimpleActionClient<pr2_controllers_msgs::PointHeadAction> PointHeadClient;
00038 
00039 RobotHead *RobotHead::instance_ = 0;
00040 
00041 RobotHead::RobotHead()
00042 {
00043     //Initialize the client for the Action interface to the head controller
00044     point_head_client_ = new PointHeadClient("/head_traj_controller/point_head_action", true);
00045 
00046     //wait for head controller action server to come up
00047     while (!point_head_client_->waitForServer(ros::Duration(5.0)) && ros::ok())
00048     {
00049         ROS_INFO("Waiting for the point_head_action server to come up");
00050     }
00051 
00052     stop = true;
00053 
00054     t1 = 0;
00055 }
00056 
00057 RobotHead::~RobotHead()
00058 {
00059     delete point_head_client_;
00060 }
00061 
00063 void RobotHead::lookAt(std::string frame_id, double x, double y, double z, bool waitfor)
00064 {
00065 
00066     if (t1)
00067        stopThread();
00068     //the goal message we will be sending
00069     pr2_controllers_msgs::PointHeadGoal goal;
00070 
00071     //the target point, expressed in the requested frame
00072     geometry_msgs::PointStamped point;
00073     point.header.frame_id = frame_id;
00074     point.point.x = x;
00075     point.point.y = y;
00076     point.point.z = z;
00077     goal.target = point;
00078 
00079     //we are pointing the wide_stereo camera frame
00080     //(pointing_axis defaults to X-axis)
00081     goal.pointing_frame = "narrow_stereo_optical_frame";
00082 
00083     //take at least 5 seconds to get there
00084     goal.min_duration = ros::Duration(2);
00085 
00086     //and go no faster than 0.1 rad/s
00087     goal.max_velocity = 0.5;
00088 
00089     //send the goal
00090     point_head_client_->sendGoal(goal);
00091 
00092     //wait for it to get there
00093     if (waitfor)
00094         point_head_client_->waitForResult();
00095 }
00096 
00097 
00098 void RobotHead::spinner(std::string frame_id, double x, double y, double z, double rate)
00099 {
00100     ros::Rate rt(rate);
00101 
00102     while (!stop && ros::ok())
00103     {
00104         rt.sleep();
00105         //the goal message we will be sending
00106         pr2_controllers_msgs::PointHeadGoal goal;
00107 
00108         //the target point, expressed in the requested frame
00109         geometry_msgs::PointStamped point;
00110         point.header.frame_id = frame_id;
00111         point.point.x = x;
00112         point.point.y = y;
00113         point.point.z = z;
00114         goal.target = point;
00115 
00116         //we are pointing the wide_stereo camera frame
00117         //(pointing_axis defaults to X-axis)
00118         goal.pointing_frame = "narrow_stereo_optical_frame";
00119 
00120         //take at least 5 seconds to get there
00121         goal.min_duration = ros::Duration(0.3);
00122 
00123         //and go no faster than 0.1 rad/s
00124         goal.max_velocity = 2.5;
00125 
00126         //send the goal
00127         point_head_client_->sendGoal(goal);
00128     }
00129 
00130     ROS_INFO("SPINNER STOPPED");
00131 
00132     t1 = 0;
00133 
00134 }
00135 
00137 void RobotHead::lookAtThreaded(std::string frame_id, double x, double y, double z)
00138 {
00139     if (t1)
00140        stopThread();
00141     stop = false;
00142     t1 = new boost::thread(&RobotHead::spinner, this, frame_id, x, y, z, 15);
00143 }
00144 
00145 void RobotHead::lookAtThreaded(std::string frame_id, btVector3 target)
00146 {
00147     if (t1)
00148        stopThread();
00149     stop = false;
00150     t1 = new boost::thread(&RobotHead::spinner, this, frame_id, target.x(), target.y(), target.z(), 15);
00151 }
00152 
00153 void RobotHead::stopThread()
00154 {
00155     ros::Rate rt(10);
00156     stop = true;
00157     while (t1) {
00158         rt.sleep();
00159     }
00160 }
00161 


ias_drawer_executive
Author(s): Thomas Ruehr
autogenerated on Mon Oct 6 2014 08:59:21