#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <nav_msgs/Odometry.h>
#include <string.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include <cmath>
int main(int argc, char** argv)
{
ros::init(argc, argv, "nav_move_base");
ros::NodeHandle node;
//订阅move_base操作服务器
actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> ac("move_base",true);
ros::Publisher odom_pub = node.advertise<nav_msgs::Odometry>("odom", 50);
ros::Time current_time, last_time;
current_time = ros::Time::now();
for(double i=-2;i<3;i++)
{
for(double j=-2;j<3;j++)
{
//设置我们要机器人走的几个点。
geometry_msgs::Point point;
geometry_msgs::Quaternion quaternion;
geometry_msgs::Pose pose_list[0];
point.x = i;
point.y = j;
point.z = 0.000;
quaternion.x = 0.000;
quaternion.y = 0.000;
quaternion.z = 0.012;
quaternion.w = 0.999;
pose_list[0].position = point;
pose_list[0].orientation = quaternion;
ROS_INFO("Waiting for move_base action server...");
//等待60秒以使操作服务器可用
if(!ac.waitForServer(ros::Duration(30)))
{
ROS_INFO("Can't connected to move base server");
return 1;
}
ROS_INFO("Connected to move base server");
ROS_INFO("Starting navigation test");
//循环通过四个航点
//初始化航点目标
move_base_msgs::MoveBaseGoal goal;
//使用地图框定义目标姿势
goal.target_pose.header.frame_id = "map";
//将时间戳设置为“now”
goal.target_pose.header.stamp = ros::Time::now();
//将目标姿势设置为第i个航点
goal.target_pose.pose = pose_list[0];
//让机器人向目标移动
//将目标姿势发送到MoveBaseAction服务器
ac.sendGoal(goal);
//等1分钟到达那里
bool finished_within_time = ac.waitForResult(ros::Duration(8));
//如果我们没有及时赶到那里,就会中止目标
if(!finished_within_time)
{
ac.cancelGoal();
ROS_INFO("Timed out achieving goal");
}
else
{
//We made it!
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
{
ROS_INFO("Goal succeeded!");
}
else
{
ROS_INFO("The base failed for some reason");
}
}
}
}
//next, we'll publish the odometry message over ROS接下来,我们将在ROS上发布odometry消息
nav_msgs::Odometry odom;
odom.header.frame_id = "odom";
//set the position
odom.pose.pose.position.x = 0.0;
odom.pose.pose.position.y = 0.0;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation.x = 0.000;
odom.pose.pose.orientation.y = 0.000;
odom.pose.pose.orientation.z = 0.000;
odom.pose.pose.orientation.w = 1.000;
//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = 0.0;
odom.twist.twist.linear.y = 0.0;
odom.twist.twist.angular.z = 0.0;
//publish the message
odom_pub.publish(odom);
ros::spin();
ROS_INFO("move_base_square.cpp end...");
return 0;
}
之前利用movebase导航定位都是通过rviz用鼠标指来指去,实验时非常方便,但实际应用总不能也人工指来指去吧,这怎么体现智能呢
启动导航后,用以前使用的rviz设设置目标点来获取map坐标系下的位置坐标
使用 2d Nav Goal 指你想要的家坐标
查看rviz终端信息
填写以下坐标: <br> geometry_msgs::PoseWithCovarianceStamped msg_poseinit;
msg_poseinit.header.frame_id = "map";
msg_poseinit.header.stamp = ros::Time::now();
msg_poseinit.pose.pose.position.x = -0.644479990005;
msg_poseinit.pose.pose.position.y = 2.2030518055;
msg_poseinit.pose.pose.position.z = 0;
msg_poseinit.pose.pose.orientation.x = 0.0;
msg_poseinit.pose.pose.orientation.y = 0.0;
msg_poseinit.pose.pose.orientation.z = -0.746261929753;
msg_poseinit.pose.pose.orientation.w = 0.665652410949;<br><br>
/*
*
*
*/
#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "std_msgs/String.h"
using namespace std;
typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
#define GOHOME "HOME"
#define GODRAMING "DRAMING"
bool Callback_flag = false;
string msg_str = "";
/*******************************默认amcl初始点******************************************/
typedef struct _POSE
{
double X;
double Y;
double Z;
double or_x;
double or_y;
double or_z;
double or_w;
} POSE;
POSE pose2 = {-8.15833854675, 3.15512728691, 0.0, 0.0, 0.0, -0.740479961141, 0.672078438241};
POSE pose1 = {-0.484616458416, 2.13149046898, 0.0, 0.0, 0.0, -0.749884700297, 0.661568542375};
POSE pose3 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
POSE pose4 = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
void setHome( ros::Publisher pub)
{
geometry_msgs::PoseWithCovarianceStamped msg_poseinit;
msg_poseinit.header.frame_id = "map";
msg_poseinit.header.stamp = ros::Time::now();
msg_poseinit.pose.pose.position.x = -0.644479990005;
msg_poseinit.pose.pose.position.y = 2.2030518055;
msg_poseinit.pose.pose.position.z = 0;
msg_poseinit.pose.pose.orientation.x = 0.0;
msg_poseinit.pose.pose.orientation.y = 0.0;
msg_poseinit.pose.pose.orientation.z = -0.746261929753;
msg_poseinit.pose.pose.orientation.w = 0.665652410949;
<br> /×因为ros话题原理本身的问题,Setting pose 需要按照以下发送×/<br>
pub.publish(msg_poseinit);
ros::Duration(1.0).sleep();
pub.publish(msg_poseinit);
ros::Duration(1.0).sleep();
pub.publish(msg_poseinit);
ros::Duration(1.0).sleep();
}
void setGoal(POSE pose)
{
//tell the action client that we want to spin a thread by default
MoveBaseClient ac("move_base", true);
//wait for the action server to come up
while(!ac.waitForServer(ros::Duration(5.0))){
ROS_WARN("Waiting for the move_base action server to come up");
}
move_base_msgs::MoveBaseGoal goal;
//we'll send a goal to the robot to move 1 meter forward
goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();
goal.target_pose.pose.position.x = pose.X;
goal.target_pose.pose.position.y = pose.Y;
goal.target_pose.pose.position.z = pose.Z;
goal.target_pose.pose.orientation.x = pose.or_x;
goal.target_pose.pose.orientation.y = pose.or_y;
goal.target_pose.pose.orientation.z = pose.or_z;
goal.target_pose.pose.orientation.w = pose.or_w;
ROS_INFO("Sending goal");
ac.sendGoal(goal);
ac.waitForResult();
if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("it is successful");
else
ROS_ERROR("The base failed move to goal!!!");
}
void poseCallback(const std_msgs::String::ConstPtr &msg)
{
ROS_INFO_STREAM("Topic is Subscriber ");
std::cout<<"get topic text: " << msg->data << std::endl;
Callback_flag = true;
msg_str = msg->data;
}
int main(int argc, char** argv)
{
ros::init(argc, argv, "base_pose_control");
ros::NodeHandle nh;
ros::Publisher pub_initialpose = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/initialpose", 10);
ros::Subscriber sub = nh.subscribe("/base/pose_topic",10,poseCallback);
//ros::Rate rate_loop(10);
setHome(pub_initialpose);
// setGoal(pose1);
while(ros::ok())
{
if(Callback_flag == true)
{
Callback_flag = false;
if(msg_str == GOHOME)
{
msg_str = "";
setGoal(pose1);
}
else if(msg_str == GODRAMING)
{
msg_str = "";
setGoal(pose2);
}
else
{
}
}
ros::spinOnce();
// rate_loop.sleep();
}
return 0;
评论(0)
您还未登录,请登录后发表或查看评论