Sending start and goal to navigation stack

asked 2020-12-06 02:38:52 -0500

Eman.m
65 ●30 ●43 ●49

updated 2020-12-06 02:40:17 -0500

I am trying to use AMCL instead of Gmapping and use pre-existing map. NOTE: I am using simulated robot(rosbot2) not a real robot.

Here is my launch file for the navigation stack configuration:

<launch>
    <arg name="use_rosbot" default="true"/>
    <arg name="use_gazebo" default="true"/>

    <param if="$(arg use_gazebo)" name="use_sim_time" value="true"/>
<!-- Bring the ROSbot model and show it in rviz  -->
 <include file="$(find rosbot_description)/launch/rosbot_rviz.launch"/>
<!-- NOTE: rosbot_rviz.launch includes: "$(find rosbot_description)/launch/rosbot.launch" which lanuches Gazebo -->

<!-- Map server -->
<arg name="map_file" default="$(find rosbot_navigation)/maps/gridMap8by8.yaml"/>
<node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" />

<!--- tf -->
<node if="$(arg use_rosbot)" pkg="tf" type="static_transform_publisher" name="laser_broadcaster" args="0 0 0 3.14 0 0 base_link laser 100" />

 <!--- Localization: Run AMCL -->
<node pkg="amcl" type="amcl" name="amcl" output="screen">
    <remap from="scan" to="/scan"/>
    <param name="odom_frame_id" value="odom"/>
    <param name="odom_model_type" value="diff-corrected"/>
    <param name="base_frame_id" value="base_link"/>
    <param name="update_min_d" value="0.1"/>
    <param name="update_min_a" value="0.1"/>
    <param name="min_particles" value="2500"/>
    <param name="max_particles" value="10000"/>
</node>

<!-- Move base -->
<node pkg="move_base" type="move_base" name="move_base" output="screen">
    <param name="controller_frequency" value="10.0"/>
    <param name="base_global_planner" value="my_newPlanner_global_planner/MyNewPlannerGlobalPlannerROS"/>
    <rosparam file="$(find rosbot_navigation)/config/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find rosbot_navigation)/config/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find rosbot_navigation)/config/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find rosbot_navigation)/config/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find rosbot_navigation)/config/trajectory_planner.yaml" command="load" />
</node>

I built this map: http://www.mediafire.com/view/fy7rsid...

This is how my map looks like in Rviz: http://www.mediafire.com/view/e3ekp50...

This is the yaml file:

image: gridMap8by8.pgm
resolution: 0.5
origin: [-2,-2, 0]
occupied_thresh: 0.65
free_thresh: 0.196
negate: 0
width: 4
height: 4

I want to set the start position of the robot at index 13 which cell (1,5), and goal is index 57 which is cell(7,1) I want to read the start and goal position from text file and use them in the above launch file. Is that possible? I don’t want to set goal from rviz, I want start and goal to be read from text file and used in the launch file.

bool makePlan (const geometry_msgs::PoseStamped &start, const geometry_msgs::PoseStamped &goal, std::vector< geometry_msgs::PoseStamped > &plan)

I implemented makePlan for new path planner, how start and goal as PoseStamped be sent to makePlan?

Appreciate your help

answered 2020-12-06 05:16:09 -0500

miura

1908 ●1 ●171 ●24 https://twitter.com/miura

The start position should be set by initial_pose_x, initial_pose_y of the amcl. If so, you can put it in the launch file. Or, I think we'll have to create a node to send the initialpose.

I think the end position should be rostopic from the launch file to send /move_base_simple/goal. https://answers.ros.org/question/4797... base_simplegoal/ is helpful. Alternatively, you'll want to create a node that sends move_base_simple/goal or move_base/goal.

edit flag offensive delete link

Comments

Thank you. In my case, base on my map above, the start is the cell (1,5), so initial_pose_x is 1 and initial_pose_y is 5, or they have to be converted to another format?

Eman.m ( 2020-12-07 13:17:29 -0500 )edit

I found these functions costmap_2d.cpp in ros_navigation: https://github.com/ericperko/ros_navi...

  void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const {
    wx = origin_x_ + (mx + 0.5) * resolution_;
    wy = origin_y_ + (my + 0.5) * resolution_;
  }

  bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const {
    if(wx < origin_x_ || wy < origin_y_)
      return false;

    mx = (int) ((wx - origin_x_) / resolution_);
    my = (int) ((wy - origin_y_) / resolution_);

    if(mx < size_x_ && my < size_y_)
      return true;

    return false;
  }

  void Costmap2D::worldToMapNoBounds(double wx, double wy, int& mx, int& my) const {
    mx = (int) ((wx - origin_x_) / resolution_);
    my = (int) ((wy - origin_y_) / resolution_);
  }

Eman.m ( 2020-12-07 14:50:52 -0500 )edit

So, for the (1,5) in occupancy, we use mapToWorld

wx = -2 + (1+0.5) * 0.5 = -1.25

wy = -2 + (5+0.5) * 0.5 = 0.75

so (1,5) is (-1.25,0.75) in the real world map, but in the map provided above (in the link) how can we see the point (-1.25,0.75)? Is it at the center of (1,5)? where is the (0,0) of the real world ?

I am so confused from the conversion results I get,,,

Eman.m ( 2020-12-07 14:51:19 -0500 )edit

Regarding initial_pose_x, initial_pose_y of the amcl, when I use them to set the position of robot:

initial_pose_x = -1.25

initial_pose_y = 0.75

In Rviz, the robot still in the center of the xy plane in Rviz

The map is shifted 1.25 meter to the right of the xy plane

and shifted 0.75 meter to the bottom of the xy plane

as in this screenshot: http://www.mediafire.com/view/gmt94v0...

Eman.m ( 2020-12-07 18:28:56 -0500 )edit

When I set: initial_pose_x = 0.75

initial_pose_y = 1.25

The robot is in the location that I wanted which is cell (1,5) as in here: http://www.mediafire.com/file/23uk6bd...

However, I need a formula so I can translate (1,5) to (0.75,1.25) ?

Eman.m ( 2020-12-07 18:54:51 -0500 )edit

instead of (1,5) I used (2,6) (index start form 1)

wx = -2 +(2+0.5) * 0.5 = -0.75 ( Here the negative gives wrong position)

wy = -2 +(6+0.5) * 0.5 = 1.25

Eman.m ( 2020-12-07 20:08:24 -0500 )edit

in the local and global parameters yaml file

I set

origin_x: -2 and origin_y: -2 (same as for map image yaml file) is that correct?

Eman.m ( 2020-12-07 20:19:34 -0500 )edit

First, change the resolution from 0.5 to 1 and, origin from [-2, -2, 0] to [0, 0, 0]. You may want to simplify the calculation and then work on it.

miura ( 2020-12-08 10:12:15 -0500 )edit

see more comments

Sending start and goal to navigation stack

1 Answer

Comments

Question Tools

Stats

Related questions

Sending start and goal to navigation stack edit

1 Answer

Comments

Question Tools

Stats

Related questions

Sending start and goal to navigation stack