#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include "tf/tf.h"
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseArray.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
class MyNode
{
public:
MoveBaseClient* ac_;
double last_x_set = 0; double last_y_set = 0; double last_th_set = 0;
void doStuff(const double& x_set, const double& y_set, const double& th_set)
{
ac_ = new MoveBaseClient("move_base",true);
while(!ac_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
move_base_msgs::MoveBaseGoal goal;
if(x_set != last_x_set || y_set != last_y_set || th_set != last_th_set)
{
goal.target_pose.header.frame_id = "base_link";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = x_set;
goal.target_pose.pose.position.y = y_set;
goal.target_pose.pose.position.z = 0;
tf::Quaternion q;
q.setRotation(tf::Vector3(0, 0, 1), th_set);
goal.target_pose.pose.orientation.x = q.x();
goal.target_pose.pose.orientation.y = q.y();
goal.target_pose.pose.orientation.z = q.z();
goal.target_pose.pose.orientation.w = q.w();
ROS_INFO("Sending goal");
ac_->sendGoal(goal,
boost::bind(&MyNode::doneCb, this, _1, _2),
MoveBaseClient::SimpleActiveCallback(),
MoveBaseClient::SimpleFeedbackCallback());
}
last_x_set = x_set; last_y_set = y_set; last_th_set = th_set;
}
void doneCb(const actionlib::SimpleClientGoalState& state,
const move_base_msgs::MoveBaseResultConstPtr& result)
{
ROS_INFO("RESULT %s",state.toString().c_str());
switch (state.state_) {
case actionlib::SimpleClientGoalState::ABORTED:
{
ROS_INFO("NavigationManager::moveBaseResultCallback: ABORTED");
}
break;
case actionlib::SimpleClientGoalState::SUCCEEDED:
{
ROS_INFO("NavigationManager::moveBaseResultCallback: SUCCEEDED");
}
break;
default:
break;
}
}
void pose_get(void)
{
if(listener_.waitForTransform("/map","/base_link",ros::Time(0),ros::Duration(0.2)))
{
listener_.lookupTransform("/map","/base_link",ros::Time(0),world_pose);
geometry_msgs::Pose pose;
pose.position.x = world_pose.getOrigin().getX();
pose.position.y = world_pose.getOrigin().getY();
pose.position.z = world_pose.getOrigin().getZ();
pose.orientation.x = world_pose.getRotation().getX();
pose.orientation.y = world_pose.getRotation().getY();
pose.orientation.z = world_pose.getRotation().getZ();
pose.orientation.w = world_pose.getRotation().getW();
ROS_INFO("position.x : %f",pose.position.x);
ROS_INFO("position.y : %f",pose.position.y);
ROS_INFO("position.z : %f",pose.position.z);
ROS_INFO("orientation.x : %f",pose.orientation.x);
ROS_INFO("orientation.y : %f",pose.orientation.y);
ROS_INFO("orientation.z : %f",pose.orientation.z);
ROS_INFO("orientation.w : %f",pose.orientation.w);
}
}
private:
tf::StampedTransform world_pose;
tf::TransformListener listener_;
};
int main (int argc, char **argv)
{
ros::init(argc, argv, "test_fibonacci_class_client");
MyNode my_node;
while(ros::ok())
{
my_node.doStuff(1,0,1);
my_node.pose_get();
ros::spinOnce();
}
return 0;
}
连续发目标点
#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <move_base_msgs/MoveBaseAction.h>
#include "tf/tf.h"
#include <tf/transform_listener.h>
#include <geometry_msgs/PoseArray.h>
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
class MyNode
{
public:
MoveBaseClient* ac_;
void init(){
ac_ = new MoveBaseClient("move_base",true);
while(!ac_->waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for the move_base action server to come up");
}
}
bool doStuff(const double& x_set, const double& y_set,const double& z_set, const double& x, const double& y, const double& z, const double& w)
{
move_base_msgs::MoveBaseGoal goal;
if(x_set != last_x_set || y_set != last_y_set)
{
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = x_set;
goal.target_pose.pose.position.y = y_set;
goal.target_pose.pose.position.z = 0;
goal.target_pose.pose.orientation.x = x;
goal.target_pose.pose.orientation.y = y;
goal.target_pose.pose.orientation.z = z;
goal.target_pose.pose.orientation.w = w;
ROS_INFO("Sending goal");
ac_->sendGoal(goal,
boost::bind(&MyNode::doneCb, this, _1, _2),
MoveBaseClient::SimpleActiveCallback(),
MoveBaseClient::SimpleFeedbackCallback());
success_ = false;
}
last_x_set = x_set; last_y_set = y_set;
return success_;
}
void doneCb(const actionlib::SimpleClientGoalState& state,
const move_base_msgs::MoveBaseResultConstPtr& result)
{
ROS_INFO("RESULT %s",state.toString().c_str());
switch (state.state_) {
case actionlib::SimpleClientGoalState::ABORTED:
{
ROS_INFO("NavigationManager::moveBaseResultCallback: ABORTED");
}
break;
case actionlib::SimpleClientGoalState::SUCCEEDED:
{
ROS_INFO("NavigationManager::moveBaseResultCallback: SUCCEEDED");
success_ = true;
}
break;
default:
break;
}
}
private:
tf::StampedTransform world_pose;
tf::TransformListener listener_;
double last_x_set ; double last_y_set ;
bool success_ = false;
};
int main (int argc, char **argv)
{
ros::init(argc, argv, "test_fibonacci_class_client");
MyNode my_node;
double pt[10][7] = {
{0.850,-10.398,0,0,0,0.795,0.606},
{1.733,-2.025,0,0,0,0.113,0.994},
{1.733,-1.492,0,0,0,0.989,-0.148},
{-0.917,-2.046,0,0,0,0.995,-0.100},
{-0.897,-1.530,0,0,0,0.115,0.993},
{1.249,-1.192,0,0,0,0.077,0.997},
{-1.371,-0.459,0,0,0,0.065,0.998},
{1.38,-0.023,0,0,0,0.065,0.998},
{1.330,0.501,0,0,0,0.996,-0.086},
{-1.966,0.059,0,0,0,0.977,-0.082}
};
my_node.init();
int ind = 0;
my_node.doStuff(pt[ind][0],pt[ind][1],pt[ind][2],
pt[ind][3],pt[ind][4],pt[ind][5],pt[ind][6]);
while(ros::ok())
{
bool ok = my_node.doStuff(pt[ind][0],pt[ind][1],pt[ind][2],
pt[ind][3],pt[ind][4],pt[ind][5],pt[ind][6]);
if(ok){
ind++;
if(ind > 9){
break;
}
}
ros::spinOnce();
}
return 0;
}