some questions about teb_local_planner_tutorials

edit

Thanks @croesmann provide repository of teb_local_planner_tutorials. When use it to simulate, i encountered some problems.

1.

when I change the parameter controller_frequency to 20, the function of bool TebLocalPlannerROS::computeVelocityCommands(geometry_msgs::Twist& cmd_vel) is not every 50ms called once. In order to printf the time interval, in computeVelocityCommands, commented out codes from

  // Shorten horizon if requested
  if (horizon_reduced_)
  {
    // reduce to 50 percent:
    int horizon_reduction = goal_idx/2;

to

  // Now visualize everything           
  planner_->visualize();
  visualization_->publishObstacles(obstacles_);
  visualization_->publishViaPoints(via_points_);
  visualization_->publishGlobalPlan(global_plan_);

and added some codes before return true:

  struct timeval start;
  double start_t,  t_diff;
  gettimeofday(&start, NULL);
  start_t = start.tv_sec*1000 + double(start.tv_usec) / 1e3;
  static double last_t=start_t;
  t_diff=start_t-last_t;
  last_t=start_t;
  ROS_INFO("time interval is %f", t_diff);

In robot_carlike_in_stage.launch, when change controller_frequency to 20, the output in the terminal is shown below :

[ INFO] [1467764473.121994736, 5.500000000]: time interval is 0.479980
[ INFO] [1467764473.221464181, 5.600000000]: time interval is 99.454102
[ INFO] [1467764473.221548948, 5.600000000]: time interval is 0.098877
[ INFO] [1467764473.321534576, 5.700000000]: time interval is 99.971191
[ INFO] [1467764473.321618796, 5.700000000]: time interval is 0.103027
[ INFO] [1467764473.420817332, 5.800000000]: time interval is 99.178955

the time interval of invoking computeVelocityCommands is not 50ms, but 0 and 99ms. When change controller_frequency to 10, it works well, the time interval of invoking computeVelocityCommands is 100ms, like below:

[ INFO] [1467764377.477714393, 6.300000000]: time interval is 98.459961
[ INFO] [1467764377.577806619, 6.400000000]: time interval is 100.093018
[ INFO] [1467764377.678022345, 6.500000000]: time interval is 100.213867
[ INFO] [1467764377.777791534, 6.600000000]: time interval is 99.770020
[ INFO] [1467764377.877468518, 6.700000000]: time interval is 99.676025

I want to change controller_frequency to 20, and the time interval of invoking computeVelocityCommands must be 50ms, what should i do ?

2

. At teb_local_planner_params.yaml, change max_vel_x to 1.5. In computeVelocityCommands, commented out codes like 1; and add some codes before return true:

cmd_vel.linear.x = 1.2; cmd_vel.linear.y = 0; cmd_vel.angular.z = 0;
but i found the robot forward 1m in 1s, not 1.2m, why? If want to set cmd_vel.linear.x large than 1m/s, what should i do ? Thanks.

• The robot_carlike_in_stage.launch is changed like below, use fake_localization replace amcl:

<launch>

    <!--  ************** Global Parameters ***************  -->
    <param name="/use_sim_time" value="true"/>

    <!--  ************** Stage Simulator ***************  -->
<node pkg="stage_ros" type="stageros" name="stageros" args="$(find teb_local_planner_tutorials)/stage/maze_carlike.world">
    <remap from="base_scan" to="scan"/>
</node>

<!--  ****** Maps *****  -->
<node name="map_server" pkg="map_server" type="map_server" args="$(find teb_local_planner_tutorials)/maps/maze.yaml" output="screen">
    <param name="frame_id" value="/map"/>
</node>

    <!--  ************** Navigation ***************  -->
<node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" command="load" ns="global_costmap" />
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/costmap_common_params.yaml" command="load" ns="local_costmap" />
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/local_costmap_params.yaml" command="load" />
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/global_costmap_params.yaml" command="load" />
    <rosparam file="$(find teb_local_planner_tutorials)/cfg/carlike/teb_local_planner_params.yaml" command="load" />

    <param name="base_global_planner" value="global_planner/GlobalPlanner" />
    <param name="planner_frequency" value ...