花生_TL007

这个屌丝很懒，什么也没留下！

热门标签

ros机器人导航设置原点，目标点_waiting to connect to move_base server

作者：花生_TL007 | 2024-04-08 10:13:25

踩

waiting to connect to move_base server

#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终端信息

填写以下坐标： 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; 

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

/*

*

*/

#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;

 　　/×因为ros话题原理本身的问题，Setting pose 需要按照以下发送×/ 

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;

}

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/花生_TL007/article/detail/384726