torso_control.cpp
Go to the documentation of this 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 }