some questions about teb_local_planner_tutorials
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 ...