wrong path provided by navigation stack [closed]

edit

Hi all!

I want to use the move_base (I am following this tutorial ) example to provide the global path (I need to test my local planner). I am running the tests on Stge.

My configuration files looks like:

    TrajectoryPlannerROS:
  max_vel_x: 0.45
  min_vel_x: 0.1
  max_rotational_vel: 1.0
  min_in_place_rotational_vel: 0.4

  acc_lim_th: 3.2
  acc_lim_x: 2.5
  acc_lim_y: 2.5

  holonomic_robot: trueobstacle_range: 7.0
raytrace_range: 3.0
footprint: [[-0.7, -0.55], [-0.7, 0.55], [0.7, 0.55], [0.7, -0.55]]
#robot_radius: ir_of_robot
inflation_radius: 0.5
observation_sources: laser_scan_sensor
#point_cloud_sensor

laser_scan_sensor: {sensor_frame: base_laser, data_type: LaserScan, topic: base_scan, marking: true, clearing: true}

#point_cloud_sensor: {sensor_frame: frame_name, data_type: PointCloud, topic: topic_name, marking: true, clearing: true}global_costmap:
  global_frame: /map
  robot_base_frame: base_link
  update_frequency: 5.0
  static_map: true
  map_type: costmaplocal_costmap:
  global_frame: odom
  robot_base_frame: base_link
  update_frequency: 2.0
  publish_frequency: 5.0
  static_map: true
  rolling_window: true
  width: 36000.0
  height: 25000.0
  resolution: 0.01

The code itself:

#include <ros ros.h="">
#include <nav_msgs getplan.h="">
#include <geometry_msgs posestamped.h="">
#include <map>
using std::map;
#include <utility>
using std::pair; using std::make_pair;
#include <boost foreach.hpp="">
#define forEach BOOST_FOREACH
#include <sys times.h="">
#include <tf tf.h="">
#include <tf transform_listener.h="">
static ros::NodeHandle* s_NodeHandle = NULL;
static ros::ServiceClient s_GetPlan;
/// Plan requests are issued using this frame - so the poses from the planner are given in this frame (e.g. map)
static std::string s_WorldFrame;
static double s_GoalTolerance = 0.5;
void navstack_init(/*int argc, char** argv*/)
{
   // get world frame
   ros::NodeHandle nhPriv("~");
   std::string tfPrefix = tf::getPrefixParam(nhPriv);
   //   s_WorldFrame = tf::resolve(tfPrefix, argv[1]);
   s_WorldFrame = tf::resolve(tfPrefix, "map");
   ROS_INFO("World frame is: %s", s_WorldFrame.c_str());

   // get goal tolerance
   char* checkPtr;
   //   s_GoalTolerance = strtod(argv[2], &checkPtr);
   s_GoalTolerance = strtod("1.0", &checkPtr);
   ROS_INFO("Goal Tolerance is: %f.", s_GoalTolerance);
   // init service query
   s_NodeHandle = new ros::NodeHandle();
   while(!ros::service::waitForService("move_base/make_plan", ros::Duration(3.0))) {
      ROS_ERROR("Service move_base/make_plan not available - waiting.");
   }

   s_GetPlan = s_NodeHandle->serviceClient<nav_msgs::getplan>("move_base/make_plan", true);
   if(!s_GetPlan) {
      ROS_FATAL("Could not initialize get plan service from move_base/make_plan (client name: %s)", s_GetPlan.getService().c_str());
   }
   ROS_INFO("Initialized Navstack Module.\n");
}

int path(){
   nav_msgs::GetPlan srv;
   srv.request.start.header.frame_id = s_WorldFrame;
   srv.request.goal.header.frame_id = s_WorldFrame;

   srv.request.start.pose.position.x = 4.0;
   srv.request.start.pose.position.y = 4.0;
   srv.request.start.pose.position.z = 0.0;
   srv.request.start.pose.orientation.x = 0.0;
   srv.request.start.pose.orientation.y = 0.0;
   srv.request.start.pose.orientation.z = 0.0;
   srv.request.start.pose.orientation.w = 1.0;

   srv.request.goal.pose.position.x = 6.0;
   srv.request.goal.pose.position.y = 4.0;
   srv.request.goal.pose.position.z = 0.0;
   srv.request.goal.pose.orientation.x = 0.0;
   srv.request.goal.pose.orientation.y = 0.0;
   srv.request.goal.pose.orientation.z = 0.0;
   srv.request.goal.pose.orientation.w = 1.0;
   srv.request.tolerance = s_GoalTolerance;

   //   double cost = INFINITE_COST;

   if(!s_GetPlan) {
      ROS_ERROR("Persistent service connection to %s failed.", s_GetPlan.getService().c_str());
      // FIXME reconnect - this shouldn't ...