part of the node's code is not executed

Question

Rosanswers logo

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&lt;geometry_msgs::PoseStamped&gt; 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(&quot;Exectution time (ms): &quot; &lt;&lt; execution_time);

    ++counter;
}while(!startCoordinatesFile.eof() &amp;&amp; !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 100" />
<!-- Publishes the voxel grid to rviz for display -->
   <node pkg="costmap_2d" type="costmap_2d_markers" name="voxel_visualizer">
    <remap from="voxel_grid" to="costmap/voxel_grid"/>
  </node>
<node name="rviz" pkg="rviz" type="rviz" />
<!-- Run the costmap node -->
  <node pkg="ros_global_planners_experiment" type="ros_global_planners_experiment_node" name="ros_global_planners_experiment_node" >
    <rosparam file="$(find ros_global_planners_experiment)/launch/ros_experiment_params.yaml" command="load"/>
  </node>
</launch>

This is the log:

... logging to /home/evadro/.ros/log/f967c6de-aac5-11eb-a0ed-e5299005fdc6/roslaunch-evadro-PowerEdge-T620-22379.log
Checking log directory for disk usage. This may take awhile.
Press Ctrl-C to interrupt
Done checking log file disk usage. Usage is <1GB.
/opt/ros/kinetic/lib/python2.7/dist-packages/roslaunch/loader.py:409: YAMLLoadWarning: calling yaml.load() without Loader=... is deprecated, as the default Loader is unsafe. Please read https://msg.pyyaml.org/load for full details.
  data = yaml.load(text)
started roslaunch server http://evadro-PowerEdge-T620:39317/
SUMMARY
PARAMETERS

/ros_global_planners_experiment_node/willowgarage_costmap/footprint: [[-0.325, -0.325]...
/ros_global_planners_experiment_node/willowgarage_costmap/footprint_padding: 0.01
/ros_global_planners_experiment_node/willowgarage_costmap/global_frame: /map
/ros_global_planners_experiment_node/willowgarage_costmap/inflation_layer/cost_scaling_factor: 10.0
/ros_global_planners_experiment_node/willowgarage_costmap/inflation_layer/inflation_radius: 0.2
/ros_global_planners_experiment_node/willowgarage_costmap/lethal_cost_threshold: 100
/ros_global_planners_experiment_node/willowgarage_costmap/map_type: costmap
/ros_global_planners_experiment_node/willowgarage_costmap/max_obstacle_height: 2.0
/ros_global_planners_experiment_node/willowgarage_costmap/obstacle_layer/inflation_radius: 0.2
/ros_global_planners_experiment_node/willowgarage_costmap/obstacle_layer/obstacle_range: 2.5
/ros_global_planners_experiment_node/willowgarage_costmap/obstacle_layer/raytrace_range: 3.0
/ros_global_planners_experiment_node/willowgarage_costmap/plugins: [{'type': 'costma...
/ros_global_planners_experiment_node/willowgarage_costmap/publish_frequency: 1.0
/ros_global_planners_experiment_node/willowgarage_costmap/robot_base_frame: base_link
/ros_global_planners_experiment_node/willowgarage_costmap/static_layer/map_topic: map
/ros_global_planners_experiment_node/willowgarage_costmap/static_map: True
/ros_global_planners_experiment_node/willowgarage_costmap/transform_tolerance: 0.3
/ros_global_planners_experiment_node/willowgarage_costmap/update_frequency: 5.0
/rosdistro: kinetic
/rosversion: 1.12.17
/use_sim_time: False

NODES
  /
    map_server (map_server/map_server)
    map_to_base_link (tf/static_transform_publisher)
    ros_global_planners_experiment_node (ros_global_planners_experiment/ros_global_planners_experiment_node)
    rviz (rviz/rviz)
    voxel_visualizer (costmap_2d/costmap_2d_markers)
auto-starting new master
process[master]: started with pid [22389]
ROS_MASTER_URI=http://localhost:11311
setting /run_id to f967c6de-aac5-11eb-a0ed-e5299005fdc6
process[rosout-1]: started with pid [22402]
started core service [/rosout]
process[map_server-2]: started with pid [22420]
process[map_to_base_link-3]: started with pid [22421]
process[voxel_visualizer-4]: started with pid [22423]
[ INFO] [1619905284.895589052]: Loading map from image "/home/evadro/Desktop/Eman/ros_workspace/src/rosbot_description/src/rosbot_navigation/maps/willowgarage-refined.pgm"
process[rviz-5]: started with pid [22439]
process[ros_global_planners_experiment_node-6]: started with pid [22450]
[ INFO] [1619905284.913307461]: Read a 1123 X 911 map @ 0.050 m/cell

Thank you,

EDIT: I added output="screen" to my node and comment out ros::spin(). I got the output from my node; however, the costmap2dROS object is not initialized with willow garage map.

In Rviz:

When, ros::spin(); was there in the code, I can see:

Map display ==> choosing Topic /ros_global_planners_experiment_node/willowgarage_costmap/costmap

But, when commenting out ros::spin(), I cannot see this topic, so I concluded that costmap2dROS object is not initialized with willow garage map.

This is Terminal output: image description

Originally posted by Eman.m on ROS Answers with karma: 65 on 2021-05-01

Post score: 0

score 0 · Accepted Answer · answered May 01 '21 at 22:12

Rosanswers logo

It works!

The edits to correct the issue are:

1- add output="screen" to my node as @robustify suggested.

2- I moved ros::spin() to the end of my node just before return 0. Thanks @robustify, you gave me a hint.

3- I added the frame_id:

start.header.frame_id = "map";

goal.header.frame_id = "map";

Originally posted by Eman.m with karma: 65 on 2021-05-01

This answer was ACCEPTED on the original site

Post score: 0

part of the node's code is not executed

SUMMARY

1 Answers1