simple_robot_control: torso_control.cpp Source File

00001 /*
00002  * torso_control.cpp
00003  *
00004  *  Created on: Jul 27, 2010
00005  *      Author: christian
00006  */
00007 
00008 
00009 #include <simple_robot_control/torso_control.h>
00010 
00011 
00012 namespace simple_robot_control{
00013 
00014 Torso::Torso():torso_client_ ("torso_controller/position_joint_action", true){
00015 
00016 
00017 
00018         //wait for the action server to come up
00019         while(!torso_client_.waitForServer(ros::Duration(5.0))){
00020                 ROS_INFO("Waiting for the torso action server to come up");
00021         }
00022 }
00023 
00024 Torso::~Torso(){
00025 
00026 }
00027 
00028 
00029 
00030 bool Torso::move(double position, bool wait)
00031 {
00032         position = (position > 0.3) ? 0.3 : position;
00033         position = (position < 0.0) ? 0.0 : position;
00034 
00035         pr2_controllers_msgs::SingleJointPositionGoal move;
00036         move.position = position;  //all the way up is 0.3
00037         move.min_duration = ros::Duration(2.0);
00038         move.max_velocity = 1.0;
00039         torso_client_.sendGoal(move);
00040 
00041         if (wait){
00042                 return torso_client_.waitForResult(ros::Duration(20.0));
00043         }
00044 
00045         return true;
00046 }
00047 
00048 }