Go to the documentation of this file.00001 #include "tibi_dabo_head_client_alg_node.h"
00002
00003 TibiDaboHeadClientAlgNode::TibiDaboHeadClientAlgNode(void) :
00004 algorithm_base::IriBaseAlgorithm<TibiDaboHeadClientAlgorithm>(),
00005 joint_motion_client_("joint_motion", true)
00006 {
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022 this->joint_motion_done=true;
00023
00024 this->joint_motion_goal_.trajectory.joint_names.resize(3);
00025 this->joint_motion_goal_.trajectory.joint_names[0]="pan";
00026 this->joint_motion_goal_.trajectory.joint_names[1]="tilt";
00027 this->joint_motion_goal_.trajectory.joint_names[2]="roll";
00028
00029 this->joint_motion_goal_.trajectory.points.resize(1);
00030
00031 this->joint_motion_goal_.trajectory.points[0].positions.resize(3);
00032 this->joint_motion_goal_.trajectory.points[0].positions[0]=0.0;
00033 this->joint_motion_goal_.trajectory.points[0].positions[1]=0.0;
00034 this->joint_motion_goal_.trajectory.points[0].positions[2]=0.0;
00035
00036 this->joint_motion_goal_.trajectory.points[0].velocities.resize(3);
00037 this->joint_motion_goal_.trajectory.points[0].velocities[0]=10.0*3.14159/180.0;
00038 this->joint_motion_goal_.trajectory.points[0].velocities[1]=10.0*3.14159/180.0;
00039 this->joint_motion_goal_.trajectory.points[0].velocities[2]=10.0*3.14159/180.0;
00040
00041 this->joint_motion_goal_.trajectory.points[0].accelerations.resize(3);
00042
00043 this->joint_motion_goal_.trajectory.points[0].time_from_start=ros::Duration(0);
00044 }
00045
00046 TibiDaboHeadClientAlgNode::~TibiDaboHeadClientAlgNode(void)
00047 {
00048
00049 }
00050
00051 void TibiDaboHeadClientAlgNode::mainNodeThread(void)
00052 {
00053
00054
00055
00056
00057
00058
00059
00060 }
00061
00062
00063
00064
00065
00066
00067 void TibiDaboHeadClientAlgNode::joint_motionDone(const actionlib::SimpleClientGoalState& state, const control_msgs::FollowJointTrajectoryResultConstPtr& result)
00068 {
00069 if( state.toString().compare("SUCCEEDED") == 0 )
00070 ROS_INFO("TibiDaboHeadClientAlgNode::joint_motionDone: Goal Achieved!");
00071 else
00072 ROS_INFO("TibiDaboHeadClientAlgNode::joint_motionDone: %s", state.toString().c_str());
00073 this->joint_motion_done=true;
00074
00075 }
00076
00077 void TibiDaboHeadClientAlgNode::joint_motionActive()
00078 {
00079
00080 }
00081
00082 void TibiDaboHeadClientAlgNode::joint_motionFeedback(const control_msgs::FollowJointTrajectoryFeedbackConstPtr& feedback)
00083 {
00084
00085
00086 bool feedback_is_ok = true;
00087
00088
00089
00090
00091
00092 if( !feedback_is_ok )
00093 {
00094 joint_motion_client_.cancelGoal();
00095
00096 }
00097 }
00098
00099
00100 void TibiDaboHeadClientAlgNode::joint_motionMakeActionRequest()
00101 {
00102 ROS_INFO("TibiDaboHeadClientAlgNode::joint_motionMakeActionRequest: Starting New Request!");
00103
00104
00105
00106 joint_motion_client_.waitForServer();
00107 ROS_INFO("TibiDaboHeadClientAlgNode::joint_motionMakeActionRequest: Server is Available!");
00108
00109
00110
00111 joint_motion_client_.sendGoal(joint_motion_goal_,
00112 boost::bind(&TibiDaboHeadClientAlgNode::joint_motionDone, this, _1, _2),
00113 boost::bind(&TibiDaboHeadClientAlgNode::joint_motionActive, this),
00114 boost::bind(&TibiDaboHeadClientAlgNode::joint_motionFeedback, this, _1));
00115 ROS_INFO("TibiDaboHeadClientAlgNode::joint_motionMakeActionRequest: Goal Sent. Wait for Result!");
00116 this->joint_motion_done=false;
00117 }
00118
00119 void TibiDaboHeadClientAlgNode::node_config_update(Config &config, uint32_t level)
00120 {
00121 this->alg_.lock();
00122 if(this->joint_motion_done)
00123 {
00124 if(config.pan)
00125 {
00126 ROS_INFO("TibiDaboHeadClientAlgNode::pan loaded!");
00127 this->joint_motion_goal_.trajectory.points[0].positions[0] = config.pan_angle*3.14159/180.0;
00128 this->joint_motion_goal_.trajectory.points[0].velocities[0] = config.speed*3.14159/180.0;
00129
00130 }
00131 if(config.tilt)
00132 {
00133 ROS_INFO("TibiDaboHeadClientAlgNode::tilt loaded!");
00134 this->joint_motion_goal_.trajectory.points[0].positions[1] = config.tilt_angle*3.14159/180.0;
00135 this->joint_motion_goal_.trajectory.points[0].velocities[1] = config.speed*3.14159/180.0;
00136
00137 }
00138 if(config.roll)
00139 {
00140 ROS_INFO("TibiDaboHeadClientAlgNode::roll loaded!");
00141 this->joint_motion_goal_.trajectory.points[0].positions[2] = config.roll_angle*3.14159/180.0;
00142 this->joint_motion_goal_.trajectory.points[0].velocities[2] = config.speed*3.14159/180.0;
00143
00144 }
00145 if(config.move)
00146 {
00147 ROS_INFO("TibiDaboHeadClientAlgNode:: requesting head move!");
00148 joint_motionMakeActionRequest();
00149 }
00150 }
00151
00152 this->alg_.unlock();
00153 }
00154
00155 void TibiDaboHeadClientAlgNode::addNodeDiagnostics(void)
00156 {
00157 }
00158
00159
00160 int main(int argc,char *argv[])
00161 {
00162 return algorithm_base::main<TibiDaboHeadClientAlgNode>(argc, argv, "tibi_dabo_head_client_alg_node");
00163 }