I have tried to search this error on forum but almost all of them had simple mistakes like a wrong link name or something. However, I am facing this error and the names seem to be correct and I do not know whether this error comes from move_base or AMCL:
Extrapolation Error: Lookup would require extrapolation into the future. Requested time 1548151625.909495115 but the latest data is at time 1548151625.886743793, when looking up transform from frame [odom] to frame [map]
[ERROR] [1548151625.962153110]: Global Frame: odom Plan Frame size 102: map
This error happens when I publish a 2d goal using RViz.
I am running a Roscore using my Ubuntu PC and also running robot_description (xacro files), robot_localization, odometry and AMCL+map_server+move_base. Basically I run everything except for sensors.
Then I run Rosclient using my robot which has IMU and Lidar. It publishes to /scan and /imu topics and also accepts cmd_vel commands and runs the motors.
This is my tf tree when I run everything:
my local_costmap params:
local_costmap:
global_frame: odom
robot_base_frame: base_footprint
update_frequency: 5.0
publish_frequency: 2.0
transform_tolerance: 5 # 0.25 seconds of latency, if greater than this, planner will stop
static_map: true
rolling_window: true # Follow robot while navigating
width: 4.0
height: 4.0
origin_x: 0 #-1.5
origin_y: 0 #-1.5
resolution: 0.03
My global_costmap params:
global_costmap:
update_frequency: 5
publish_frequency: 2
transform_tolerance: 5 # 0.25 seconds of latency, if greater than this, planner will stop
static_map: true
global_frame: map
robot_base_frame: base_footprint
rolling_window: true # Follow robot while navigating
track_unknown_space: true
width: 32 #27.2
height: 32 #30.4
origin_x: -7
origin_y: -7
resolution: 0.03 #0.05
plugins:
- {name: static_layer, type: "costmap_2d::StaticLayer"}
- {name: obstacle_layer, type: "costmap_2d::ObstacleLayer"}
- {name: inflation_layer, type: "costmap_2d::InflationLayer"}
↧
Extrapolation Error
↧
global_planner crashes when orientation mode is any of interpolation types?
Hi,
I am using ROS Kinetic on Ubuntu 16.04, and am not sure if I have wrongly configured any of the files, or **whether this occurs regardless of the configuration?**. Any thoughts/experiences in this regard would be helpful.
I have repeatedly observed the following for the global_planner planning in the navigation stack for both, move_base and move_base_flex:
When an invalid (in my case, inaccessible) goal was given to the planner with the orientation mode set to either None or Forward, the planner simply reports an error "Failed to get a plan." and continues its retries/resets, however configured.
However, with all the remaining configuration set same, but the orientation mode switched to either Interpolate or Forward then Interpolate, the planner reports the "Failed to get a plan." error once and then crashes the move_base/move_base_flex node.
↧
↧
DWB local planner configuration
Hi everyone,
I have some issues while trying to use DWB local planner.
I'm using it with move_base using [this answer]( https://answers.ros.org/question/312524/how-to-use-the-dwb-local-planner/?answer=312527#post-id-312527).
But when I start the navigation, I get these errors:
[ WARN] [1464825005.450915057]: Critic "ObstacleFootprint" failed to prepare
[ERROR] [1464825005.457386149]: computeVelocityCommands exception: No valid trajectories out of 63!
[ERROR] [1464825005.550973984]: Footprint spec is empty, maybe missing call to setFootprint?
I've tried to add in my "base_local_planner_params.yaml" file the "ObstacleFootprint" parameter :
critics:
ObstacleFootprint: [[-0.3, 0.45], [0.97, 0.45], [0.97, -0.45], [-0.3,-0.45]]
But now, my robot goes around in circles with no errors instead of moving towards the destination...
Thanks for your help!
↧
DWA local planner problem when an obstacle is detected on the goal
Dear all,
I am facing a problem that I do not know how to fix it.
I use the [move_base/global_planner](http://wiki.ros.org/global_planner) as the *global_planner* and the [DWA_local_planner](http://wiki.ros.org/dwa_local_planner) as the *local_planner*.
In order to have the full control of the trajectory I have divided the trajectory into lany short trajectory by using some *WayPoints*. As you can see in the following picture, when there is and obstacle on on waypoint, the *DWA_local_planner* completely stops till the point would be free (the obstacles moves). I have put a very large radius for each waypoints (around 2.5 m) so as you see in the picture, the */move_base/DWAPlannerROS/Global_planner* already sends the trajectory to the next Waypoint but the robot completely stops.
By using the [TEB_local_planner](http://wiki.ros.org/teb_local_planner) as the *local_planner* the problem is solved but I want (for certain reasons) use the *DWA_local_planner*.
I was wondering if anyone can help me to understand how to fix this problem.
Thanks,
↧
How to send multiple waypoints to move_base package?
I am using ROS Indigo and I am publishing 5 waypoints from one computer to a ROS machine with turtlebot simulator (ROS/JAUS Bridge). The message I got on my ROS computer is as below
`header: seq:3340 stamp: secs:5244 nsecs:11300000 frame_id:base_link poses: - header: seq:0 stamp: secs:5244 nsecs:113000000, frame_id:base_link Pose: Position : x: 9.999 y: 9.999 z: 0.000 orientation: x:3.5 y:4 z: 0.0 w:1.0` . Like this, I have 5 different messages (5 waypoints). I need our move_base package accept this message (topic name is **cmd_local_way_paths**) and need to drive the robot through the waypoints one by one. Thanks in advance for the help. The screenshot of the terminal with the messages are attached
[C:\fakepath\1.png](/upfiles/15482924723577464.png)
Thanking you
Melvin
↧
↧
DWB_local_planner - Distance between the robot and the obstacle
Dear all,
I use the [DWB_local_planner](https://github.com/locusrobotics/robot_navigation/tree/master/dwb_local_planner) and the robot gets very close to the obstacle while avoiding it.
I was wondering if you know how to configure the *DWB_local_planner* that the robot keeps a specific distance from the obstacle. In the *DWB_local_planner* parameter, as far as I understood, a specific parameter has not been dedicated for this issue.
I know that by configuring the [local_costmap](http://wiki.ros.org/costmap_2d) (as for example has been explained [here](https://answers.ros.org/question/12874/costmap2d-inflation-radius/) ) I can force the robot to keep a certain distance from the obstacle but I prefer to do have an override to this.
Thanks,
↧
Writing a custom path using 'Move Base Flex Exe Path Action'
I would like to send a series of poses to Move Base, and have my robot follow these poses. I am using [Move Base Flex](http://wiki.ros.org/move_base_flex), which extends a few extra actions compared to the regular Move Base. I am currently trying to understand how to use [ExePathAction](https://github.com/magazino/move_base_flex/blob/master/mbf_msgs/action/ExePath.action).
From what I understand, it allows the controller (i.e. the base local planner) to follow a set of poses. I am trying to give it 5 poses each 0.1 cm apart. What I've written so far:
#include "ros/ros.h"
#include
#include
#include
#include
#include
#include
typedef actionlib::SimpleActionClient PathClient;
int main(int argc, char** argv){
ros::init(argc, argv, "simple_path");
PathClient pc("mbf_costmap_nav/exe_path", true); // true doesnt need ros::spin
while(!pc.waitForServer(ros::Duration(5.0))){
ROS_INFO("Waiting for Move Base server to come up");
}
mbf_msgs::ExePathGoal target_path_;
//move_base_msgs::MoveBaseGoal goal;
nav_msgs::Path path_;
path_.header.frame_id = "base_link";
path_.poses[0].pose.position.x = 0.1;
path_.poses[0].pose.position.y = 0.0;
path_.poses[0].pose.orientation.z = 0.0;
path_.poses[0].pose.orientation.w = 1.0;
path_.header.frame_id = "base_link";
path_.poses[1].pose.position.x = 0.2;
path_.poses[1].pose.position.y = 0.0;
path_.poses[1].pose.orientation.z = 0.0;
path_.poses[1].pose.orientation.w = 1.0;
path_.header.frame_id = "base_link";
path_.poses[2].pose.position.x = 0.3;
path_.poses[2].pose.position.y = 0.0;
path_.poses[2].pose.orientation.z = 0.0;
path_.poses[2].pose.orientation.w = 1.0;
path_.header.frame_id = "base_link";
path_.poses[3].pose.position.x = 0.4;
path_.poses[3].pose.position.y = 0.0;
path_.poses[3].pose.orientation.z = 0.0;
path_.poses[3].pose.orientation.w = 1.0;
path_.header.frame_id = "base_link";
path_.poses[4].pose.position.x = 0.5;
path_.poses[4].pose.position.y = 0.0;
path_.poses[4].pose.orientation.z = 0.0;
path_.poses[4].pose.orientation.w = 1.0;
target_path_.path = path_;
pc.sendGoal(target_path_);
pc.waitForResult();
if(pc.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("Base moved %s", pc.getState().toString().c_str());
else if(pc.getState() == actionlib::SimpleClientGoalState::ABORTED)
ROS_INFO("Goal aborted");
else
ROS_INFO("Base failed to move for some reason");
return 0;
}
However this doesn't execute. I looked at the [legacy relay node](https://github.com/magazino/move_base_flex/blob/master/mbf_costmap_nav/scripts/move_base_legacy_relay.py) which relays classic move_base goals that appear on the move_base_simple/goal topic. However, that's not what I am looking for. I would like to directly use the ExePathAction, as I want to program more sophisticated navigation behaviour in the future.
↧
using move_base recovery for initial pose estimation
Hi,
I want my robot to find its initial pose on its own rather than providing it with rviz "2d pose estimate". For that I already figured out that you can call AMCLs /global_localization service, whicht distributes the particles over the whole map. Second I'd need to rotate the robot. Therefore move_base already has the recovery behaviour, but how to trigger that? I know there is the C++ API, but isn't there a service call for that? Something that smoothly integrates with move_base resp. nav stack? Or can I use the actionlib for that? Anything any trigger despite the cpp api?
I'm wondering for a long time why there isn't something out there for initial pose estimation with the widely used combination of amcl and move_base. Are you guys all using the rviz-button? all the time you start demos?
**Update 1**
As I could not find an answer to this, I wrote a node triggering a recovery-behaviour similar to the [move_base.cpp](https://github.com/strawlab/navigation/blob/master/move_base/src/move_base.cpp) implementation. However those behaviours ([rotate_recovery](http://wiki.ros.org/rotate_recovery), [clear_costmap_recovery](http://wiki.ros.org/clear_costmap_recovery)) need the global and local costmap as parameters. Just creating your own as suggested in the code snipples from the wiki doesnt seem to work as intended since those maps are not the ones used by move_base: Running *clear_costmap_recovery* then does not clear the costmap used by move_base. **However**, and this is strange, *rotate_recovery* sometimes is not carried out due to potential collision. How can there be collisions if the costmap is empty? Or IS there a connection between a costmap you create and name ('local_costmap', 'global_costmap') and the ones used by move_base? This is confusing ...
An alternative solution would be to use *actionlib* and send *move_base_msgs/MoveBaseActionGoal*, but how can I tell move_base to just use the local planner and ignore the global one?
↧
How can I cancel or skip a goal using actionlib, upon getting a keyboard input ?
I am using simple_navigation_goal package for sending number of waypoints(say 6 waypoints) to the move_base. I am reading the goals from a .csv file and moving the robot to the goal one by one. But I need to skip/ cancel a goal, when I hit any key from he keyboard. I know that I have to use public member function `cancelGoal ()` to achieve this.Could you please help me how can I achieve this by giving a sample code?
Thanks
↧
↧
Why does move_base abort this path?
I'm using frontier_exploration to do autonomous mapping and this is the first goal that it sends the robot when it is initialized.
The robot can't move and I get these errors:
[ERROR] [1459373069.392635517]: Footprint spec is empty, maybe missing call to setFootprint?
[ WARN] [1459373069.393029517]: Invalid Trajectory 0.000000, 0.000000, 0.400000, cost: -9.000000
[ WARN] [1459373069.393286183]: Rotation cmd in collision
[ INFO] [1459373069.394777850]: Error when rotating.
[ INFO] [1459373069.490389850]: Got new plan
Eventually, all of the recovery behaviors fail and I get the error:
[ERROR] [1459373090.449866595]: Aborting because a valid control could not be found. Even after executing all recovery behaviors
[ERROR] [1459373090.450383095]: Failed to move
This can't be a problem with the costmaps because there aren't any obstacles nearby.
Any thoughts?
↧
Mini-Turty robot get stuck on corner when navigating
I created a map using a Rhoeby Dynamics Mini-Turty II robot and am using that to do navigation. It works most of the time, but sometimes it gets "stuck" on corners. As far as I can determine, it seems to create a global path that is too close to the obstacle (eg. corner of a wall). Then when it navigates along the path, it appears to just barely enter the "lethal" area and then gives up (goes into recovery behaviors). Is there a way to have the global path take a wider route around obstacles?
The robot is using Ubuntu 16.04 on Raspberry Pi 3, Kinetic, Move Base, DWA planner and AMCL.
↧
how Could I get a parameter from the parameter server and use it in .yaml file.
What I am trying to do is to install two turtlebots in GAZEBO in reference to this famous
[post](https://answers.ros.org/question/41433/multiple-robots-simulation-and-navigation/) and try to cover willow garage with them.
I managed to get AMCL running for both robots just fine. Now I need to set move base but I want obviously to set all the costmaps running for the namespace of each robot. So According to the code bellow I set the tf prefixes (robo1_tf/ , robot2_tf).
So what I have is one name space here and a tf_prefix:
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: base_link, data_type: LaserScan, topic: scan, marking: true, clearing: true, expected_update_rate: 15.0}
so What I wanna do, instead of creating two seperate costmap files going along the line
common_costmap_params1.yaml
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: robot1_tf/base_link, data_type: LaserScan, topic: robot1/scan, marking: true, clearing: true, expected_update_rate: 15.0}
common_costmap_params2.yaml
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: robot2_tf/base_link, data_type: LaserScan, topic: robot2/scan, marking: true, clearing: true, expected_update_rate: 15.0}
I want to keep ONE common_costmap_params.yaml for BOTH namespaces like
observation_sources: laser_scan_sensor
laser_scan_sensor: {sensor_frame: $(tf_prefix)/base_link, data_type: LaserScan, topic: $(ns)/scan, marking: true,
clearing: true, expected_update_rate: 15.0}
if it was python code I Would do
`prefix = rospy.get_param("tf_prefix")`
But How do I do the above python trick in a yaml file?
I do have read the [Param Server](http://wiki.ros.org/Parameter%20Server) docs but there sth I cannot gasp
something in the Syntax I do not know. I thought that move_base would work out of the box for each ns but this does not seem to be the case
If you need any list of topics, nodes, tf_tree, node graph, let me know and I will update.
If the question is not well stated I will update also.
thanks for any direction or hint or link in advance.
↧
Unable to avoid obstacles & robot can't go in a straight line using ros-kinetic, move_base, and SLAM_gmapping.
Attempting to use the ros navigation stack for autonomy. I'm able to get Laserscan information in rviz, but unable to avoid obstacles using the rplidar, SLAM_gmapping, and move_base. When giving the robot a 2d nav_goal in rviz the robot shifts right and left then fails to go in a straight line. Any feedback would be appreciated. Most of the necessary files are below!
gmapping.launch
↧
↧
How is possible to modify manually occupancy grid cells on a map?
So the point is that during the mapping with a mobile robot I will get a set of coordinates during the process. I was wondering, how is possible to modify the values of the occupancy grid cells nearby such coordinates.
For instance, these coordinates would represent objects that I don't want them in the map but they have been detected and stored in the map during the gmapping process as obstacles. I am not sure how to approach, whether using a real-time processing or post-processing.
Is there any available tool that can achieve something similar?
↧
A global planner using a Roadmap graph
Is there a global planner in ROS1 that can deal with a predefined Roadmap graph to make the robot move on its edges.
I think this would be a very useful thing but can not find anything in that area.
↧
IWR1443 Radar cannot work well on UAV
Sorry first, still familiarizing with ROS and might not understand some terminology. Currently working on final year project which uses Texas Instrument IWR1443 Radar and using the ROS package they provide. This package, however, is built for TurtleBot 2. So, my main task is to make it work with MavROS and remove as much as the TurtleBot package.
I was able to get the Mapping Example Package given by TI running after some minor modification but still working on modifying the navigation example package. There is 2 main issue now.
- Obstacle detection:
The object detected by radar is displayed as PointCloud points. The obstacle mapping only maps points at a certain altitude and azimuth angle range, and only turns the PointCloud into obstacles in CostMap if the point exceeds a certain intensity threshold. Ther result can be seen via RVIS
However, no matter how low I set the threshold, I cannot get the cardboard boxes (the obstacle) to be detected at 100% accuracy. It only detected sometimes. Even, more solid objects (metal chair) still failed to detect sometimes. It does appear to be able to detect obstacle better when it is on the ground, hold by hand steadily, and not flying. Is it because it requires steady altitude or movement in general?
- Move Base Navigation
The TI navigation package uses move base. I was able to write a package to redirect the move base velocity topic (/cmd_vel) to MavROS velocity topic (/mavros/setpoint_velocity/cmd_vel_unstamped). However, when I am run the navigation package and use the simple goal example (http://wiki.ros.org/navigation/Tutorials/SendingSimpleGoals), the UAV seems to be inconsistent. The best I can observe is once the goal is sent, the UAV will make a left (always) to the desired heading and stay there, not moving forward at all, not reaching the target. I gives these warnings and info:
[ WARN]: mmWave customized rotate recovery behavior started.
[ WARN]: Clearing costmaps...
[ WARN]: Map update loop missed its desired rate of 20.0000Hz... the loop actually took 0.1053 seconds
[ WARN]: Map update loop missed its desired rate of 15.0000Hz... the loop actually took 0.1471 seconds
[ WARN]: Performing rotation...
[ WARN]: Completed rotation...
[ INFO]: Got new plan
[ WARN]: Control loop missed its desired rate of 5.0000Hz... the loop actually took 60.3261 seconds
[ INFO]: Got new plan
[ INFO]: Got new plan
[ INFO]: Got new plan (20 more lines)
I have been trying to work on this issue for more than a month and now I feel very lost. I will provide as much information as I can. But please, help me!
↧
Control Loop error
This is sort of the continuation of my [previous post](https://answers.ros.org/question/317399/iwr1443-radar-cannot-work-well-on-uav/). I was able to get a more reliable obstacle detection by lowering the resolution. However, I am still confused on why I get a control loop error. While I seem to find some post regarding this problem, all of them due to a slight time delay. While in my case, the delay is always more than 20 seconds and up to 60 seconds.
What happened during that time is every time I specified a goal on RVIZ, instead immediately aiming for the desired path, it will perform a 360-degree rotation first to the desired heading. Then it just hover in place. After the rotation, these errors always come up.
> [ INFO]: Got new plan>> [ WARN]: Control loop missed its desired rate of 5.0000Hz... the loop actually took 60.3261 seconds>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan>> [ INFO]: Got new plan
It will not stop until I force the package to stop. I don't know what cause this behavior and I cannot found anyone else with a similar problem.
↧
↧
move_base action server returning SUCCEEDED immediately after sending goal
In my turtlebot simulation, I have a queue of `move_base_mgs::MoveBaseGoal` that I send to move_base's action server via an action client node I wrote. Upon completing an action the next one is sent, until the queue is empty, thus completing a predefined path. Quite often however, the server will return a SUCCEEDED goal state immediately upon sending the goal.
Here's the simplified c++-code:
class ActionClient{
actionlib::SimpleActionClient client;
std::queue goals;
public: void goalCallback (const actionlib::SimpleClientGoalState& state){
if (!goals.empty()){
move_base_msgs::MoveBaseGoal next = goals.front();
goals.pop();
client.sendGoal(next, boost::bind(&SquarePathClient::goalCallback, this, _1),
actionlib::SimpleActionClient::SimpleActiveCallback());
}
}
public: ActionClient() : client("/move_base", true) {
client.waitForServer();
// Fill queue;
client.sendGoal(firstGoal, boost::bind(&ActionClient::goalCallback, this, _1),
actionlib::SimpleActionClient::SimpleActiveCallback());
}
int main (int argc, char **argv){
ros::init(argc, argv, "actionclient_node");
ActionClient ac;
ros::Rate r(100);
while (ros::ok()){
ros::spinOnce();
r.sleep();
}
return 0;
}
The tolerances are set very tightly, so the goals are definitely not actually reached. I was thinking that the SUCCEEDED state might still be a leftover from the previous goal, thus triggering two callbacks, as the issue appears to occur mostly on every second goal sent, but I have found no way to check this.
↧
Move_base stops responding after first goal
I'm attempting to create a program that can send a series of waypoints to a turtlebot3 performing 2D slam with AMCL and using move_base as the controller. Doing all this as a simulation.
I'm trying to get this all running in gazebo. I have followed the [turtlebot3 navigation and simulation tutorials](http://emanual.robotis.com/docs/en/platform/turtlebot3/simulation/#turtlebot3-simulation-using-gazebo), I can get gazebo running, see the robot, launch Rviz, reset the pose, and even send a command via the Rviz 2N nav goal GUI, and it all works.
First I run:
roslaunch turtlebot3_gazebo turtlebot3_world.launch
Then:
roslaunch turtlebot3_navigation turtlebot3_navigation.launch map_file:=$HOME/map.yaml
However, when I go to give it a second goal (either from terminal, or the rviz GUI), nothing happens. No new global or local plan is generated, it just sits there.
Ive tried running rostopic echo move_base/status:
for the first goal, everything works fine and when it reaches goal I get this:
header:
seq: 1104
stamp:
secs: 285
nsecs: 411000000
frame_id: ''
status_list:
-
goal_id:
stamp:
secs: 2690710792
nsecs: 32577
id: "/move_base-1-2690710792.32577"
status: 3
text: "Goal reached."
When I set a new 2nd goal, it seems to be immediately rejected because another goal?
header:
seq: 1308
stamp:
secs: 326
nsecs: 8000000
frame_id: ''
status_list:
goal_id:
stamp:
secs: 2690710792
nsecs: 32577
id: "/move_base-1-2690710792.32577"
status: 3
text: "Goal reached."
goal_id:
stamp:
secs: 321
nsecs: 25000000
id: "/move_base-2-321.25000000"
status: 8
text: "This goal was canceled because another goal was recieved by the simple action server"
Ive dug through as many of these rostopic lists as I could looking for more clues, but have come up empty handed. Please hlep me shed light on this one!!
A few notes on system:
ROS Kinetic running on Ubuntu 16.04, all on a virtual machine running in windows.
↧
different between rospy and roscpp on move_base_simple/goal
hi there:
recently, i have been working on publishing goals with move_base_simple/goal to make my robot move around directly. yesterday, i wrote something in C, trying to make the robot move from (0,0) - (3,0) - (0,0) - (0,4), but after it got (3,0), (0,0) was published, but before the robot got the origin place, i got the "goal reached" message from /move_base/result, which leading the robot move to the next goal, (0,4) i mean. today, i try the same thing in rospy, but everything works fine. can anyone tell me what is going on? btw, i am using ros kinetic with turtlebot, and i got the same question on turtlebot_gazebo.
here are the codes:
#include
#include
#include
#include
//ros publisher
ros::Publisher target_pub;
float place[4][2] = {{0.0, 1.0},{0.0, 0.0},{1.0, 0.0},{0.0, 0.0}};
int i = 0, j = 0;
void result_callback(const move_base_msgs::MoveBaseActionResult::ConstPtr &msg)
{
if(msg->status.status == 3)
i=i+1;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "test_node");
ros::NodeHandle n;
ros::Subscriber result_sub = n.subscribe("/move_base/result", 10, result_callback);
target_pub = n.advertise("/move_base_simple/goal", 10);
ros::Rate loop_rate(1);
while (ros::ok())
{
if (i == 0)
{
geometry_msgs::PoseStamped msg;
msg.header.seq = j++;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "map";
msg.pose.position.x = place[0][0];
msg.pose.position.y = place[0][1];
msg.pose.position.z = 0.0;
msg.pose.orientation.w = 1.0;
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
target_pub.publish(msg);
}
else if (i < 4)
{
geometry_msgs::PoseStamped msg;
msg.header.seq = j++;
msg.header.stamp = ros::Time::now();
msg.header.frame_id = "map";
msg.pose.position.x = place[i][0];
msg.pose.position.y = place[i][1];
msg.pose.position.z = 0.0;
msg.pose.orientation.w = 1.0;
msg.pose.orientation.x = 0.0;
msg.pose.orientation.y = 0.0;
msg.pose.orientation.z = 0.0;
target_pub.publish(msg);
}
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
#!/usr/bin/env python
import rospy
from move_base_msgs.msg import MoveBaseActionResult
from geometry_msgs.msg import PoseStamped, Pose
from std_msgs.msg import Header
i = 0
j = 0
place = [[0.0, 1.0],[0.0, 0.0],[1.0, 0.0],[0.0, 0.0]]
def callback(data):
global i
if data.status.status == 3:
i=i+1
rospy.init_node('test_node', anonymous=True)
rospy.Subscriber("/move_base/result", MoveBaseActionResult, callback)
pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=10)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
if i == 0:
msg = PoseStamped()
msg.header.seq = j
j=j+1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.pose.position.x = place[0][0]
msg.pose.position.y = place[0][1]
msg.pose.position.z = 0.0
msg.pose.orientation.w = 1.0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.0
pub.publish(msg)
elif i<4:
msg = PoseStamped()
msg.header.seq = j
j=j+1
msg.header.stamp = rospy.Time.now()
msg.header.frame_id = "map"
msg.pose.position.x = place[i][0]
msg.pose.position.y = place[i][1]
msg.pose.position.z = 0.0
msg.pose.orientation.w = 1.0
msg.pose.orientation.x = 0.0
msg.pose.orientation.y = 0.0
msg.pose.orientation.z = 0.0
pub.publish(msg)
rate.sleep()
↧