part of the node's code is not executed

edit

Hi

I implemented a node to create a costmap2dROS from map server. Then use this costmap2dROS object to initialize NavfnROS object. I want to measure the run time of makePlan.

The code for creating costmap2dROS from map server works perfectly as I can see the costmap in Rviz. However, the code from the line: ROS_INFO("Start using willow_garage_costmap to experiment with navFn global planner"); does not work as I cannot see this ROS_INFO in the terminal or any ROS_INFO after it.

This is my node: ros_global_planners_experiment_node.cpp

#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>
#include <navfn/navfn_ros.h>
#include<iostream>
#include <fstream>

using namespace std;

int main (int argc, char** argv)
{
  // Announce this program to the ROS master as a "node" called "ros_global_planners_experiment_node"
  ros::init(argc, argv,"ros_global_planners_experiment_node");


  //ros::NodeHandle n("~");

  tf::TransformListener tf_(ros::Duration(10));
  costmap_2d::Costmap2DROS willow_garage_costmap("willowgarage_costmap", tf_);

  ros::spin();

  ROS_INFO("Start using willow_garage_costmap to experiment with navFn global planner");

  ros::WallTime begin_, end_;

  double start_world_x; 
  double start_world_y; 
  double goal_world_x; 
  double goal_world_y;

  // input(read) file
  string path = "/home/evadro/Desktop/Eman/ros_workspace/src/ros_global_planners_experiment/src/";
  // each line of this text file contains two double representing startX, startY
  string startFileName = path + "ros_start_world_coordinates_random_scenarios.txt";
  ifstream startCoordinatesFile;
  startCoordinatesFile.open(startFileName,ios::in);
  // each line of this text file contains two double representing goalX, goalY
  string goalFileName = path + "ros_goal_world_coordinates_random_scenarios.txt";
  ifstream goalCoordinatesFile;
  goalCoordinatesFile.open(goalFileName,ios::in);

  navfn::NavfnROS navfn;
  navfn.initialize("my_navfn_planner", &willow_garage_costmap);

  int counter = 1;

  if(! startCoordinatesFile || ! goalCoordinatesFile)
  {
        ROS_INFO("Unable to open the files");
  }//end if
  else
  {
    do{
        ROS_INFO("Scenario #: %d",counter);
        // read double start_world_x, start_world_y, then extract and throw away white space
        startCoordinatesFile >> start_world_x >> start_world_y >> std::ws  ;
        // read double goal_world_x, goal_world_y, then extract and throw away white space
        goalCoordinatesFile >> goal_world_x >> goal_world_y >> std::ws  ;

        geometry_msgs::PoseStamped start;
        start.header.stamp = ros::Time::now();
        start.pose.position.x = start_world_x;
        start.pose.position.y = start_world_y;
        start.pose.position.z = 0.0;

        geometry_msgs::PoseStamped goal;
        goal.header.stamp = ros::Time::now();
        goal.pose.position.x = goal_world_x;
        goal.pose.position.y = goal_world_y;
        goal.pose.position.z = 0.0;

        std::vector<geometry_msgs::PoseStamped> plan;

        begin_ = ros::WallTime::now();
        navfn.makePlan(start,goal,plan);
        end_ = ros::WallTime::now();

        double execution_time = (end_ - begin_).toNSec() * 1e-6;
        ROS_INFO_STREAM("Exectution time (ms): " << execution_time);

        ++counter;
    }while(!startCoordinatesFile.eof() && !goalCoordinatesFile.eof());

    startCoordinatesFile.close();
    goalCoordinatesFile.close();
  }//end else

  return 0;
}//end main

This is the launch file:

<launch>
  <!-- 
  NOTE: You'll need to bring up something that publishes sensor data (see
  rosstage), something that publishes a map (see map_server), and something to
  visualize a costmap (see nav_view), to see things work.
  Also, on a real robot, you'd want to set the "use_sim_time" parameter to false, or just not set it.
  -->
  <param name="/use_sim_time" value="false"/>

  <!-- Map server -->
    <arg name="map_file" default="$(find rosbot_navigation)/maps/willowgarage-refined.yaml"/>
    <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_file)" respawn="false" output="screen"/>

  <node pkg="tf" type="static_transform_publisher" name="map_to_base_link" args="0 0 0 0 0 0 /map base_link ...