Move_base fails

[abhy]

Hello,

1. We are trying to set a goal from our code as stated in the tutorial.
When we send a goal from other machine, it takes the goal and shows arrow in
the rviz but it does not plan a path to traverse.
When we gave goal from rviz directly it always plans a path.

The code is like below,

goal.target_pose.header.frame_id = "map";
goal.target_pose.header.stamp = ros::Time::now();


goal.target_pose.pose.position.x = msg->position.x;
goal.target_pose.pose.position.y = msg->position.y;
goal.target_pose.pose.orientation.w = 1.0;


ROS_INFO("Sending goal");
ac.sendGoal(goal);

ac.waitForResult();

if(ac.getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
ROS_INFO("Hooray, Goal reached");
else
ROS_INFO("The base failed to move");


Also, To give multiple goals one after another we tried to set a ros::ok()
while loop but at first goal itself it does not stop at ac.waitForResult()
and continues loop no matter what the result is.


2. I have another question with AMCL. Do we need to set an initial pose
using PoseWithCovarianceStamped message type? or does it takes a position
initially automatically?
When we executed rviz it sometimes shows pointcloud and sometimes not. when
it is not then manually we have to set a position in rviz.
Or, does it detects robot position automatically using sensor readings and
initiates?

- Abhy

[Brian Gerkey]

Post by abhy
2. I have another question with AMCL. Do we need to set an initial pose
using PoseWithCovarianceStamped message type? or does it takes a position
initially automatically?
When we executed rviz it sometimes shows pointcloud and sometimes not. when
it is not then manually we have to set a position in rviz.
Or, does it detects robot position automatically using sensor readings and
initiates?

amcl needs an initial pose estimate. There are 2 sources for it:

* parameters: `~initial_pose_*` and `~initial_pose_*` determine the
initial distribution when amcl starts up. You might set these
parameters in a .launch file if your robot always starts in the same
location (common in simulation, not with real robots). If they're not
set, amcl takes a default distribution around the pose (0,0,0). And
when it's running, amcl periodically writes the current estimate into
those parameters, so if you restart amcl without restarting roscore,
amcl will take the last saved pose as its initial estimate.

* topic: publish a PoseWithCovarianceStamped message to `initialpose`
at any time to give amcl a pose estimate. This is most commonly done
by clicking in rviz, because it's easy to visually check the result
(e.g., by comparing rendered laser scans with the map). But you can
send this message from any node.

brian.

[abhy]

Thanks brian for the reply. So, i can set parameters through x,y,a.

We are still struggling with the move_base issue. we can set the target
properly but planner does not plans a path. Surprisingly, sometimes when
target is set path is getting planned but most of the times it fails to do
so. But, it is not the case with rviz. Through rviz when target is set local
planner runs well.

Are we doing something wrong?

[Eitan Marder-Eppstein]

Abhy,

Am I understanding correctly that, at this point, sending a goal using rviz
works just fine for you, but that sending a goal to the navigation stack
with code does not?

The code you posted above looks reasonable, are you sure that the location
you're attempting to send the robot to is valid? You can visualize the
current goal that the navigation stack is pursuing via the "current_goal"
topic described here:
http://www.ros.org/wiki/navigation/Tutorials/Using%20rviz%20with%20the%20Navigation%20Stack#Current_Goal.
You should see that goal show up in rviz in free space for the navigation
stack to have a reasonable shot at succeeding. Also, the navigation stack
attempts to tell you why it fails to reach its goal. Can you bring up a
console (rxconsole) and post any output you get from move_base on failure?
That should help diagnose exactly what's going on.

Hope this helps,

Eitan

Post by abhy
Thanks brian for the reply. So, i can set parameters through x,y,a.
We are still struggling with the move_base issue. we can set the target
properly but planner does not plans a path. Surprisingly, sometimes when
target is set path is getting planned but most of the times it fails to do
so. But, it is not the case with rviz. Through rviz when target is set local
planner runs well.
Are we doing something wrong?
--
http://ros-users.122217.n3.nabble.com/Move-base-fails-tp2369343p2382327.html
Sent from the ROS-Users mailing list archive at Nabble.com.
_______________________________________________
ros-users mailing list
https://code.ros.org/mailman/listinfo/ros-users

[abhy]

Hello Eitan,

Yes, you are right!

We are trying to achieve to send goals to the robot one after another. In
tutorial it can set only a single goal.

We had tried couple of thing to work this out:

1. Put while(ros::ok()) loop around move_base tutorial code and check if
old destination <> new_destination, if yes then send_goal to Action server.
like, below:

while(ros::ok())
{
if goal.x <> old_goal and same for y then,
{goal.x = <x from outside>
goal.y = <y from outside>
send_goal and wait for feedback;}
old_goal = goal.x and same for y.
ros::spin();
}

Here, we found that it does not stop for a goal to achieve, while loop
continues its execution. We also tried with threads.

2. In next trial, we removed the code from while loop and kept code as it
is. Here, we gave a call to move_base code from some outside code which is
receiving destinations x,y.
like,

Code1: receive new x,y other than old if yes call move_base program.
and in move_base_program: same code given in the tutorial.

Even this did not work.

3. we also explored your rviz goal.cpp file where we found you are
populating data in poseStamped directly. So we tried to copy the same
fundamental in our code which also did not help.

Every time we are getting below message after trying all above option,
"Aborting because a valid plan could not be found. Even after executing all
recovery behaviors"
The interesting part is that, it is setting an arrow against destination in
rviz but not planning a path towards it.

4. Apart from all above options finally, we tried finally to give goals one
after another from rviz. In this case it worked properly (arrow set and path
is planned). We tried to give same goals which were there in first through
first options.
My rxconsole is taking lot of time to execute so i am not able to get the
output from it.


Long story short, It is setting an arrow from code or directly from rviz but
it does not draw a line of local planner from robot to destination if it is
through our code.

Are we suppose work on any other point?

-Abhy

[Eitan Marder-Eppstein]

Abhy,

Post by abhy
Hello Eitan,
Yes, you are right!
We are trying to achieve to send goals to the robot one after another. In
tutorial it can set only a single goal.
1. Put while(ros::ok()) loop around move_base tutorial code and check if
old destination <> new_destination, if yes then send_goal to Action server.
while(ros::ok())
{
if goal.x <> old_goal and same for y then,
{goal.x = <x from outside>
goal.y = <y from outside>
send_goal and wait for feedback;}
old_goal = goal.x and same for y.
ros::spin();
}
Here, we found that it does not stop for a goal to achieve, while loop
continues its execution. We also tried with threads.

Looping around a call to send_goal_and_wait should do what you want, the
code should block until the goal is completed and then allow for the next
goal to be sent. I suspect that the real problem is that move_base is
aborting on the goals that you send to it for some reason.

Post by abhy
2. In next trial, we removed the code from while loop and kept code as it
is. Here, we gave a call to move_base code from some outside code which is
receiving destinations x,y.
like,
Code1: receive new x,y other than old if yes call move_base program.
and in move_base_program: same code given in the tutorial.
Even this did not work.
3. we also explored your rviz goal.cpp file where we found you are
populating data in poseStamped directly. So we tried to copy the same
fundamental in our code which also did not help.
Every time we are getting below message after trying all above option,
"Aborting because a valid plan could not be found. Even after executing all
recovery behaviors"
The interesting part is that, it is setting an arrow against destination in
rviz but not planning a path towards it.

The message you've posted above is the navigation stack telling you that it
cannot achieve your goal and is aborting on it. Specifically, the global
planner cannot create a plan to your goal point. This is likely because the
goal point is in an obstacle. Can you look in rviz and check to see if the
goal point is in an obstacle, inflated obstacle or unknown space? A tutorial
on using rviz with the navigation stack can be found here:
http://www.ros.org/wiki/navigation/Tutorials/Using%20rviz%20with%20the%20Navigation%20Stack

Post by abhy
4. Apart from all above options finally, we tried finally to give goals one
after another from rviz. In this case it worked properly (arrow set and path
is planned). We tried to give same goals which were there in first through
first options.
My rxconsole is taking lot of time to execute so i am not able to get the
output from it.

Are you sure that the goals you are giving in rviz are the same as those
that are coming from code? It seems like the difference between the goals is
that the ones you're sending via rviz are valid, meaning the navigation
stack can create a plan to them, while the ones that you're sending from
code are not.

Post by abhy
Long story short, It is setting an arrow from code or directly from rviz but
it does not draw a line of local planner from robot to destination if it is
through our code.

You're not seeing a plan from code because the navigation stack is unable to
create a plan and aborts on the goal... so this is expected.

Hope this helps,

Eitan

Post by abhy
Are we suppose work on any other point?
-Abhy
--
http://ros-users.122217.n3.nabble.com/Move-base-fails-tp2369343p2395552.html
Sent from the ROS-Users mailing list archive at Nabble.com.
_______________________________________________
ros-users mailing list
https://code.ros.org/mailman/listinfo/ros-users

[abhy]

Thanks Eitan for your response.

I have ensured that goal is not being set in unknown or somewhere in the
obstacle.
When i click on rviz the global planner is working properly but from code it
is not. In both ways global planner is referring same configuration files
of the stack. As i said it is setting a goal but not planning a path to move
on it.

Is there anything i should specifically look for?

Abhy.

[Eitan Marder-Eppstein]

Abhy,

Strange, I was almost sure that would be the problem. OK, a couple things to
try:

1) Since you can't run rxconsole, run "rostopic echo move_base/status" and
then send a goal... do you see any useful messages in the "text" field of
the messages? Something about why move_base aborts?

2) Can you "rostopic echo move_base/goal" and "rostopic echo
move_base_simple/goal" both when you send a goal from rviz and when you send
a goal from code. Do you notice any significant difference between the two
poses? The best way to do this would be to send a goal through code and then
send a goal in rviz where you line up the goal sending arrow with the arrow
produced by the goal sent from code. Rviz sends a raw PoseStamped and the
action client sends an action goal, I'm just interested in how the
PoseStamped in that goal matches the PoseStamped sent by rviz.

Let me know how those suggestions go, I'll also try to think up more reasons
you could be seeing this behavior.

Hope all is well,

Eitan

Post by abhy
Thanks Eitan for your response.
I have ensured that goal is not being set in unknown or somewhere in the
obstacle.
When i click on rviz the global planner is working properly but from code it
is not. In both ways global planner is referring same configuration files
of the stack. As i said it is setting a goal but not planning a path to move
on it.
Is there anything i should specifically look for?
Abhy.
--
http://ros-users.122217.n3.nabble.com/Move-base-fails-tp2369343p2404593.html
Sent from the ROS-Users mailing list archive at Nabble.com.
_______________________________________________
ros-users mailing list
https://code.ros.org/mailman/listinfo/ros-users

[abhy]

Hello Eitan,

I did check topics you have mentioned. Lately i found that when i launch nav
stack at first time /initial_pose is not getting published in spite of
sending x,y,a through parameters in amcl.

I always have to click in rviz to set its pose and target.

in short, i have to set initial pose and then set goal at least for the
first time from rviz and then only from the next time stack takes goal from
the code.

Is there any way by which i can publish amcl pose properly at first instance
to get pointcloud? so that i do not need to set goal from rviz for the first
time.

Any guidance on this would be appreciated.

abhy.

[Eitan Marder-Eppstein]

Abhy,

Post by abhy
Hello Eitan,
I did check topics you have mentioned. Lately i found that when i launch nav
stack at first time /initial_pose is not getting published in spite of
sending x,y,a through parameters in amcl.

Can you post the results of what you saw from the checks I suggested? The
output might actually help us figure out what's going on.

Post by abhy
I always have to click in rviz to set its pose and target.

You can write code that publishes on the "initialpose" topic, but that will
only work if the robot starts in about the same place in your map every time
it comes up. Typically, we just set the initial pose in rviz once when we
bring our robots up.

Post by abhy
in short, i have to set initial pose and then set goal at least for the
first time from rviz and then only from the next time stack takes goal from
the code.

Are you saying that once you set the pose of the robot in rviz things start
working for you? If that's the case then, yes, the robot must be localized
before navigation can be expected to work properly. This means you need to
set the initialpose of the robot, either from code or in rviz, properly
before you send goals to the navigation stack.

Post by abhy
Is there any way by which i can publish amcl pose properly at first instance
to get pointcloud? so that i do not need to set goal from rviz for the first
time.

As I mentioned above, you can do it from code by sending a
PoseWithCovarianceStamped message (
http://www.ros.org/doc/api/geometry_msgs/html/msg/PoseWithCovarianceStamped.html)
to amcl on the "initialpose" topic. However, be careful with this because it
means you'll have to make sure that the robot always starts in the correct
position in the world. I'd suggest just setting the "initialpose" in rviz on
startup... or writing some code to solve the kidnapped robot problem since
we haven't gotten around to it yet... patches are always welcome ;)

Hope this helps,

Eitan

Post by abhy
Any guidance on this would be appreciated.
abhy.
--
http://ros-users.122217.n3.nabble.com/Move-base-fails-tp2369343p2411161.html
Sent from the ROS-Users mailing list archive at Nabble.com.
_______________________________________________
ros-users mailing list
https://code.ros.org/mailman/listinfo/ros-users

[abhy]

Hello Eitan,

Sorry for the late reply on this. So, far i am still testing with the
move_base and initial pose.

According to your pose, it is clear that we have to publish an initial pose
of the robot to make it localize. But, It wont be easy in our case to set a
position from rviz since we have created an interface (Custom) to control
the robot remotely written in java. so, i found a package ar_pose which i
managed to test it properly.

Here my approach is, to fixed the pattern on ceiling and x,y,z of it
accordingly and let the robot camera identify its initial pose comparing
distance. publish it through on initial topic. This i tested and I could
able to do get reference x,y,z of robot but i am still not getting point
cloud in rviz.

I just wanted to know if this approach is fine and why it is still not
publishing point cloud in first launch when initial pose topic is being
published propely.

-Abhy

[Eitan Marder-Eppstein]

Abhy,

Post by abhy
Hello Eitan,
Sorry for the late reply on this. So, far i am still testing with the
move_base and initial pose.
According to your pose, it is clear that we have to publish an initial pose
of the robot to make it localize. But, It wont be easy in our case to set a
position from rviz since we have created an interface (Custom) to control
the robot remotely written in java. so, i found a package ar_pose which i
managed to test it properly.
Here my approach is, to fixed the pattern on ceiling and x,y,z of it
accordingly and let the robot camera identify its initial pose comparing
distance. publish it through on initial topic. This i tested and I could
able to do get reference x,y,z of robot but i am still not getting point
cloud in rviz.
I just wanted to know if this approach is fine and why it is still not
publishing point cloud in first launch when initial pose topic is being
published propely.

Your approach seems fine in principle. Perhaps something is awry with the
initialpose message you're sending. Are you sure that its in the right
frame? (You probably want "map") Can you rostopic echo initialpose when you
send it and post the output? You might also want to echo it when you set the
pose from rviz and check to see if there are any obvious differences.

Hope this helps,

Eitan

Post by abhy
-Abhy
--
http://ros-users.122217.n3.nabble.com/Move-base-fails-tp2369343p2440479.html
Sent from the ROS-Users mailing list archive at Nabble.com.
_______________________________________________
ros-users mailing list
https://code.ros.org/mailman/listinfo/ros-users

[abhy]

Hello Eitan,

Thanks for your help. I tried working on it on the other day. And found that
i was not providing the cov. matrix to the initial pose. I took the help of
rviz pose setting .cpp file and provided the same to it. It worked by
showing pose array.

Now as i said my plan is to attach a pattern on ceiling and position robot
after completing a job approximately under the pattern. When the robot will
restart again then it will detect the position by ar_pose pattern match and
figure it out in the map. Here my pattern position is fixed, Z axis in robot
is 0 but pattern will show z value due to height which then i will neglect
and take only x and y w.r.t pattern to transmit initial position.

I hope my approach is fine. I request you to help me in case of any
suggestion or if i am wrong.

-Abhy

[Eitan Marder-Eppstein]

Abhy,

Glad things are working now. I think your marker-based approach seems
reasonable for pose initialization.

Hope all is well,

Eitan

Post by abhy
Hello Eitan,
Thanks for your help. I tried working on it on the other day. And found that
i was not providing the cov. matrix to the initial pose. I took the help of
rviz pose setting .cpp file and provided the same to it. It worked by
showing pose array.
Now as i said my plan is to attach a pattern on ceiling and position robot
after completing a job approximately under the pattern. When the robot will
restart again then it will detect the position by ar_pose pattern match and
figure it out in the map. Here my pattern position is fixed, Z axis in robot
is 0 but pattern will show z value due to height which then i will neglect
and take only x and y w.r.t pattern to transmit initial position.
I hope my approach is fine. I request you to help me in case of any
suggestion or if i am wrong.
-Abhy
--
http://ros-users.122217.n3.nabble.com/Move-base-fails-tp2369343p2473301.html
Sent from the ROS-Users mailing list archive at Nabble.com.
_______________________________________________
ros-users mailing list
https://code.ros.org/mailman/listinfo/ros-users