0
$\begingroup$

Rosanswers logo

Hi, I have wrote the following package totally the same as tutorials:robot_setup_tf,laser_scan_publisher,odometry_publisher. And I also wrote the following base controller package but when I run by roslaunch, it seems not call the callback. the base controller code:

  #include <ros/ros.h>
  #include <geometry_msgs/Twist.h>
  #include <iostream>
  
  using namespace std;
  
  void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel);
  
  int main(int argc,char **argv)
  {
    cout << "123" << endl;
    ros::init(argc, argv, "robot_setup_base");
      ros::Subscriber cmd_vel_sub;
      ros::NodeHandle node_;
      cmd_vel_sub =       node_.subscribe<geometry_msgs::Twist>("robot_0/cmd_vel", 1,       boost::bind(cmdVelReceived,_1));
      ros::spin();
    return 0;
  }
  
  void cmdVelReceived(const geometry_msgs::Twist::ConstPtr& cmd_vel)
  {
    cout << "x=" << cmd_vel->linear.x << endl;
    cout << "y=" << cmd_vel->linear.y << endl;
    cout << "theta=" << cmd_vel->angular.z << endl;
  }

And my launch file is

  <launch>
  
    <master auto="start"/>
    <param name="/use_sim_time" value="true"/>  
  
    <node pkg="robot_setup_laser" type="laser"       name="sensor_node_name" output="screen">
      <param name="sensor_param" value="param_value" />
    </node>
  
    <node pkg="robot_setup_odometry" type="odometry"       name="odom_node" output="screen">
      <param name="odom_param" value="param_value" />
    </node>
  
    <node pkg="robot_setup_tf" type="tf_broadcaster"       name="transform_configuration_name" output="screen">
      <param name="transform_configuration_param" value="param_value"       />
    </node>
  
    <node pkg="robot_setup_base" type="base" name="base"       output="screen">
      <param name="transform_configuration_param" value="param_value"       />
    </node>
  
    <!--- Run AMCL -->
    <include file="$(find amcl)/examples/amcl_diff.launch" />
  
    <node pkg="map_server" type="map_server" name="map_server"       args="$(find navigation_stage)/stage_config/maps/willow-full.pgm 0.1"       respawn="false" >
      <param name="frame_id" value="/map" />
    </node>
  
    <node pkg="stage" type="stageros" name="stageros" args="$(optenv       ROS_STAGE_GRAPHICS -g) $(find       navigation_stage)/stage_config/worlds/willow-pr2-multi.world" respawn="false">
      <param name="base_watchdog_timeout" value="0.2"/>
    </node>
  
    <!-- Throttle the voxel grid that is being published for rviz -->
    <node ns="move_base_node/local_costmap"       name="voxel_grid_throttle" pkg="topic_tools" type="throttle" args="messages       voxel_grid 3.0 voxel_grid_throttled" />
  
    <group ns="robot_0">
      <param name="tf_prefix" value="robot_0" />
      <node pkg="move_base" type="move_base" respawn="false"       name="move_base_node" output="screen">
        <remap from="map" to="/map" />
        <param name="controller_frequency" value="10.0" />
        <rosparam file="$(find       navigation_stage)/move_base_config/costmap_common_params.yaml"       command="load" ns="global_costmap" />
        <rosparam file="$(find       navigation_stage)/move_base_config/costmap_common_params.yaml"       command="load" ns="local_costmap" />
        <rosparam file="$(find       navigation_stage)/move_base_config/local_costmap_params.yaml"       command="load" />
        <rosparam file="$(find       navigation_stage)/move_base_config/global_costmap_params.yaml"       command="load" />
        <rosparam file="$(find       navigation_stage)/move_base_config/base_local_planner_params.yaml"       command="load" />
      </node>
  
  
        <node pkg="fake_localization" type="fake_localization"       name="fake_localization" respawn="false">
      </node>
  
    </group>
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find       navigation_stage)/multi_robot.vcg" />
  </launch>


  And the output:

  sam@sam-M51Kr:~/code/ros/ira_navigation/nav_core/launch$ roslaunch       robot_conf.launch 
  ... logging to       /home/sam/.ros/log/e84ff146-891a-11e0-93d5-0015afe5d6b9/roslaunch-sam-M51      Kr-4359.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.
  
  WARNING: ignoring defunct <master /> tag
  started roslaunch server http://sam-M51Kr:41097/
  
  SUMMARY
  ========
  
  PARAMETERS
   * /use_sim_time
   * /amcl/gui_publish_rate
   *       /robot_0/move_base_node/TrajectoryPlannerROS/min_in_place_rotational_vel
   * /base/transform_configuration_param
   * /robot_0/move_base_node/TrajectoryPlannerROS/y_vels
   * /robot_0/move_base_node/local_costmap/unknown_threshold
   * /amcl/recovery_alpha_slow
   * /amcl/laser_z_short
   * /robot_0/move_base_node/local_costmap/origin_y
   * /robot_0/move_base_node/TrajectoryPlannerROS/min_vel_x
   * /robot_0/move_base_node/TrajectoryPlannerROS/sim_time
   * /robot_0/move_base_node/TrajectoryPlannerROS/heading_lookahead
   * /robot_0/move_base_node/global_costmap/base_scan/data_type
   * /robot_0/move_base_node/global_costmap/static_map
   * /amcl/laser_z_rand
   * /robot_0/move_base_node/global_costmap/mark_threshold
   * /robot_0/move_base_node/local_costmap/mark_threshold
   * /robot_0/move_base_node/local_costmap/height
   * /robot_0/move_base_node/local_costmap/base_scan/marking
   * /robot_0/move_base_node/global_costmap/base_scan/marking
   * /robot_0/move_base_node/local_costmap/z_resolution
   * /robot_0/move_base_node/TrajectoryPlannerROS/vx_samples
   * /robot_0/move_base_node/global_costmap/raytrace_range
   * /robot_0/move_base_node/local_costmap/origin_x
   * /robot_0/move_base_node/local_costmap/origin_z
   * /robot_0/move_base_node/TrajectoryPlannerROS/sim_granularity
   * /robot_0/move_base_node/local_costmap/observation_sources
   * /robot_0/move_base_node/local_costmap/lethal_cost_threshold
   * /robot_0/move_base_node/global_costmap/footprint
   * /transform_configuration_name/transform_configuration_param
   * /amcl/laser_lambda_short
   *       /robot_0/move_base_node/local_costmap/base_scan/observation_persistence
   * /robot_0/move_base_node/local_costmap/resolution
   * /amcl/laser_max_beams
   * /robot_0/move_base_node/TrajectoryPlannerROS/goal_distance_bias
   * /amcl/laser_model_type
   *       /robot_0/move_base_node/global_costmap/base_scan/min_obstacle_height
   * /robot_0/move_base_node/local_costmap/publish_frequency
   * /amcl/odom_model_type
   * /amcl/laser_sigma_hit
   * /amcl/recovery_alpha_fast
   * /robot_0/move_base_node/TrajectoryPlannerROS/escape_vel
   * /robot_0/move_base_node/global_costmap/update_frequency
   * /robot_0/move_base_node/local_costmap/inflation_radius
   * /robot_0/move_base_node/global_costmap/footprint_padding
   * /robot_0/move_base_node/local_costmap/footprint_padding
   * /robot_0/move_base_node/TrajectoryPlannerROS/prune_plan
   * /robot_0/move_base_node/global_costmap/inflation_radius
   * /rosdistro
   * /robot_0/move_base_node/global_costmap/global_frame
   * /robot_0/move_base_node/TrajectoryPlannerROS/vtheta_samples
   * /robot_0/move_base_node/local_costmap/robot_base_frame
   * /robot_0/move_base_node/local_costmap/global_frame
   * /map_server/frame_id
   * /robot_0/move_base_node/local_costmap/static_map
   * /amcl/odom_alpha4
   * /robot_0/move_base_node/local_costmap/transform_tolerance
   * /amcl/odom_alpha1
   * /amcl/odom_alpha2
   * /amcl/odom_alpha3
   * /robot_0/move_base_node/global_costmap/cost_scaling_factor
   * /robot_0/move_base_node/TrajectoryPlannerROS/dwa
   * /robot_0/move_base_node/global_costmap/z_resolution
   * /robot_0/move_base_node/global_costmap/observation_sources
   * /amcl/odom_alpha5
   * /stageros/base_watchdog_timeout
   *       /robot_0/move_base_node/local_costmap/base_scan/min_obstacle_height
   * /robot_0/move_base_node/local_costmap/width
   * /robot_0/move_base_node/TrajectoryPlannerROS/acc_lim_y
   * /robot_0/move_base_node/TrajectoryPlannerROS/acc_lim_x
   * /robot_0/move_base_node/global_costmap/rolling_window
   * /sensor_node_name/sensor_param
   * /amcl/max_particles
   * /amcl/laser_z_max
   * /robot_0/move_base_node/local_costmap/update_frequency
   * /robot_0/move_base_node/local_costmap/publish_voxel_map
   * /amcl/min_particles
   * /robot_0/move_base_node/local_costmap/base_scan/data_type
   * /robot_0/move_base_node/local_costmap/z_voxels
   * /robot_0/move_base_node/global_costmap/origin_z
   * /robot_0/move_base_node/TrajectoryPlannerROS/max_vel_x
   * /robot_0/move_base_node/global_costmap/publish_frequency
   * /robot_0/move_base_node/TrajectoryPlannerROS/xy_goal_tolerance
   * /robot_0/move_base_node/global_costmap/z_voxels
   *       /robot_0/move_base_node/local_costmap/base_scan/max_obstacle_height
   * /robot_0/move_base_node/controller_frequency
   * /robot_0/move_base_node/local_costmap/base_scan/clearing
   * /robot_0/move_base_node/TrajectoryPlannerROS/oscillation_reset_dist
   * /robot_0/move_base_node/global_costmap/obstacle_range
   * /amcl/transform_tolerance
   * /robot_0/move_base_node/global_costmap/base_scan/clearing
   * /amcl/kld_err
   * /amcl/resample_interval
   * /amcl/odom_frame_id
   * /robot_0/move_base_node/global_costmap/unknown_threshold
   * /robot_0/move_base_node/TrajectoryPlannerROS/yaw_goal_tolerance
   * /robot_0/move_base_node/TrajectoryPlannerROS/max_rotational_vel
   * /robot_0/move_base_node/TrajectoryPlannerROS/occdist_scale
   * /robot_0/move_base_node/global_costmap/lethal_cost_threshold
   * /amcl/laser_likelihood_max_dist
   * /amcl/laser_z_hit
   * /robot_0/move_base_node/local_costmap/rolling_window
   * /amcl/update_min_d
   * /robot_0/move_base_node/global_costmap/max_obstacle_height
   * /robot_0/move_base_node/local_costmap/obstacle_range
   * /robot_0/move_base_node/local_costmap/cost_scaling_factor
   * /robot_0/move_base_node/global_costmap/robot_base_frame
   * /robot_0/move_base_node/TrajectoryPlannerROS/holonomic_robot
   *       /robot_0/move_base_node/global_costmap/base_scan/expected_update_rate
   * /robot_0/tf_prefix
   * /robot_0/move_base_node/local_costmap/map_type
   * /rosversion
   * /robot_0/move_base_node/local_costmap/footprint
   * /robot_0/move_base_node/local_costmap/raytrace_range
   * /robot_0/move_base_node/TrajectoryPlannerROS/path_distance_bias
   * /odom_node/odom_param
   * /amcl/kld_z
   *       /robot_0/move_base_node/local_costmap/base_scan/expected_update_rate
   * /amcl/update_min_a
   * /robot_0/move_base_node/global_costmap/map_type
   *       /robot_0/move_base_node/global_costmap/base_scan/max_obstacle_height
   * /robot_0/move_base_node/global_costmap/transform_tolerance
   * /robot_0/move_base_node/TrajectoryPlannerROS/acc_lim_th
   *       /robot_0/move_base_node/global_costmap/base_scan/observation_persistence
   * /robot_0/move_base_node/local_costmap/max_obstacle_height
  
  NODES
    /move_base_node/local_costmap/
      voxel_grid_throttle (topic_tools/throttle)
    /robot_0/
      move_base_node (move_base/move_base)
      fake_localization (fake_localization/fake_localization)
    /
      sensor_node_name (robot_setup_laser/laser)
      odom_node (robot_setup_odometry/odometry)
      transform_configuration_name (robot_setup_tf/tf_broadcaster)
      base (robot_setup_base/base)
      amcl (amcl/amcl)
      map_server (map_server/map_server)
      stageros (stage/stageros)
      rviz (rviz/rviz)
  
  auto-starting new master
  process[master]: started with pid [4373]
  ROS_MASTER_URI=http://localhost:11311
  
  setting /run_id to e84ff146-891a-11e0-93d5-0015afe5d6b9
  process[rosout-1]: started with pid [4386]
  started core service [/rosout]
  process[sensor_node_name-2]: started with pid [4389]
  process[odom_node-3]: started with pid [4390]
  process[transform_configuration_name-4]: started with pid [4391]
  process[base-5]: started with pid [4392]
  123
  process[amcl-6]: started with pid [4393]
  process[map_server-7]: started with pid [4394]
  process[stageros-8]: started with pid [4403]
  process[move_base_node/local_costmap/voxel_grid_throttle-9]: started       with pid [4418]
  process[robot_0/move_base_node-10]: started with pid [4454]
  process[robot_0/fake_localization-11]: started with pid [4457]
  process[rviz-12]: started with pid [4493]
  [ INFO] [1306580950.572397882]: Requesting the map...
  [ WARN] [1306580950.608350236]: Request for map failed; trying again...
  [ INFO] [1306580952.582282085, 1.000000000]: Received a 584 X 526 map       @ 0.100 m/pix
  
  [ INFO] [1306580952.628130027, 1.000000000]: Initializing likelihood field       model; this can take some time on large maps...
  [ INFO] [1306580953.041931902, 1.500000000]: Done initializing likelihood       field model.
  [ WARN] [1306580953.516881476, 2.000000000]: Message from       [/sensor_node_name] has a non-fully-qualified frame_id [laser_frame]. Resolved       locally to [/laser_frame].  This is will likely not work in multi-robot systems.  This       message will only print once.
  [ INFO] [1306580959.034326825, 7.500000000]: Subscribed to Topics:       base_scan
  [ INFO] [1306580959.131188791, 7.600000000]: Requesting the map...
  
  [ INFO] [1306580959.174969713, 7.600000000]: Still waiting on map...
  
  [ INFO] [1306580960.148279766, 8.600000000]: Still waiting on map...
  
  [ INFO] [1306580961.164992342, 9.600000000]: Received a 584 X 526 map       at 0.100000 m/pix
  
  [ INFO] [1306580962.145220653, 10.600000000]: MAP SIZE: 584, 526
  [ INFO] [1306580962.242705462, 10.700000000]: Subscribed to Topics:       base_scan
  [ INFO] [1306580962.737211056, 11.200000000]: Sim period is set to 0.10
  [ WARN] [1306580963.423087563, 11.900000000]: The base_scan       observation buffer has not been updated for 0.50 seconds, and it should be T      updated every 0.40 seconds.
  [ WARN] [1306581053.524506038, 102.000000000]: MessageFilter       [target=/odom ]: Dropped 100.00% of messages so far. Please turn the       [ros.amcl.message_notifier] rosconsole logger to DEBUG for more information.

I have test

  rostopic pub /cmd_vel geometry_msgs/Twist -r 20 '[1,0,0]' '[0,0,0.5]'

And it actually output x=1 y=0 theta=0.5

I have three simple question:

  1. The origin purpose is try to get the output from the navigation stack. How could I get the velocity from the navigation stack with cmd_vel?

  2. Why the output from '[1,0,0]' '[0,0,0.5]' is [1,0,0.5] ?

  3. What is this message means?

    Waiting on transform from /base_footprint to /odom to become available before running costmap, tf error: Frame id /base_footprint does not exist!

By the way,I have update the launch file and output result.

Now my launch file can run the navigation simulation with fixed map, and I also can get the cmd_vel like:

  x=0.1
  y=0
  theta=0.0336842
  x=0.1
  y=0
  theta=0.0168421
  x=0.1
  y=0
  theta=0
  x=0.1
  y=0
  theta=0.0505263
  x=0.1
  y=0
  theta=0.0673684

It's seems great~~^^

My question:

  1. When I change /use_sim_time to false,why the robot just disappeared? Should I also can get cmd_vel from robot_0/cmd_vel? or I should use /cmd_vel?

  2. How to use the real robot's laser data to replace the simulation laser data in Rviz?

  3. How to dynamic build map when robot moving, not to use the static map?

Thank you~~

Originally posted by sam on ROS Answers with karma: 2570 on 2011-05-26

Post score: 0

Improve this question
$\endgroup$

1 Answer 1

Reset to default
0
$\begingroup$

Rosanswers logo

In order for your cmd_vel callback to be called, you will have to have some node actually publishing to the cmd_vel topic. This could be a teleoperation node if you are driving around manually with a joystick or an autonomous controller like move_base, which is part of the ROS Navigation stack.

To really simply test your code, you could try the following in another terminal while your base controller is running:

rostopic pub /cmd_vel geometry_msgs/Twist -r 20 '[1,0,0]' '[0,0,0.5]'

That will publish a Twist message corresponding to 1 meter/s of forwards motion and 0.5 rad/s of yaw at a rate of 20Hz.

You can also verify that a publisher is working by doing:

rostopic echo /cmd_vel

If you don't see any output from rostopic echo, then you should not expect a callback on the topic you echoed to be called.

Update to address followup questions

I'll address each individually:

  1. A properly configured move_base setup will output commands on the cmd_vel topic, as I'm guessing you expected. However, you have some tf errors that I'll address later that likely are preventing the navigation stack from getting very far in it's initialization process (or otherwise causing it to not output velocity commands). To check if the navigation stack is outputting velocity commands, you use rostopic echo as mentioned above. If I recall correctly, you'll also have to give the navigation stack a goal before it will output any commands. You can do that from rviz pretty easily; see the tutorial Using rviz with the Navigation Stack for details on how to setup a good visualization to use while running navigation.

  2. A geometry_msgs/Twist contains 6 fields (once expanded; see the online documentation for exact field names), hence the 6 different input values for rostopic pub (details on that command line syntax can be found at the YAML Commandline wiki page). Since you only output 3 of those 6 values in your callback (linear.x, linear.y, angular.z), you only see 3 values per message in the output from your node.

  3. It means that, as you have it configured, the navigation stack (specifically the costmaps) expect there to be a transform available between the /odom reference frame and the /base_footprint frame. If you are using different names for either of those reference frames, your configuration files will need to be updated to reflect that. You can see what available paths there are in the transform tree by running

    rosrun tf view_frames

in a terminal while the rest of your code (that should be providing your required tf frames) is running. If you are still unsure about how to fix it after looking at the output of view_frames (a file called frames.pdf in the directory you ran view_frames from), please update your answer to include both frames.pdf and your config files (or links to those files).

Originally posted by Eric Perko with karma: 8406 on 2011-05-26

This answer was ACCEPTED on the original site

Post score: 1

Improve this answer
$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.