simple_robot_control: simple_robot_control: head

00001 /*
00002  * head_control.cpp
00003  *
00004  *  Created on: Jul 27, 2010
00005  *      Author: christian
00006  */
00007 
00008 #include <tf/transform_datatypes.h>
00009 #include <simple_robot_control/head_control.h>
00010 
00011 namespace simple_robot_control{
00012 
00013 Head::Head(){
00014           traj_client_ = new actionlib::SimpleActionClient< pr2_controllers_msgs::PointHeadAction> ("head_traj_controller/point_head_action", true);
00015 
00016           if (!traj_client_->waitForServer(ros::Duration(5.0))){
00017                   ROS_ERROR("Head: Could not find the joint_trajectory_action server");
00018            }
00019 }
00020 Head::~Head(){
00021         delete traj_client_;
00022 }
00023 
00024 
00025 
00026 
00027 bool Head::lookat(  const string source_frame, tf::Vector3 position , double speed, string pointing_frame ){
00028 
00029 
00030         pr2_controllers_msgs::PointHeadGoal goal;
00031         goal.pointing_frame = pointing_frame;
00032         goal.pointing_axis.x = 1;
00033         goal.pointing_axis.y = 0;
00034         goal.pointing_axis.z = 0;
00035 
00036         goal.target.header.frame_id = source_frame;
00037         goal.target.point.x = position.x();
00038         goal.target.point.y = position.y();
00039         goal.target.point.z = position.z();
00040 
00041     goal.min_duration = ros::Duration(0.5);
00042     goal.max_velocity = speed;
00043 
00044         traj_client_->sendGoal(goal);
00045         bool success = traj_client_->waitForResult(ros::Duration(10.0));
00046         if (!success){
00047                 ROS_ERROR("Could not point head");
00048                 return false;
00049         }
00050         ROS_INFO("moved head");
00051         return true;
00052 
00053 }
00054 
00055 
00056 
00057 bool Head::lookat(  const  geometry_msgs::PoseStampedConstPtr pose, double speed, string pointing_frame){
00058 
00059         return lookat(pose->header.frame_id, tf::Vector3(pose->pose.position.x, pose->pose.position.y, pose->pose.position.z), speed, pointing_frame);
00060 }
00061 
00062 
00063 
00064 
00065 bool Head::lookat( const string source_frame, double x, double y, double z, double speed, string pointing_frame ){
00066 
00067         return lookat(source_frame, tf::Vector3(x,y,z), speed, pointing_frame);
00068 }
00069 
00070 
00071 bool Head::lookat( const tf::StampedTransform  pose, double speed, string pointing_frame){
00072 
00073         return lookat(pose.frame_id_, pose.getOrigin(), speed, pointing_frame);
00074 }
00075 
00076 
00077 }