Robot stack in simulation

asked 2020-02-21 04:36:13 -0500

lorenzo gravatar image

updated 2020-02-21 05:52:05 -0500

I am trying to run a simulation with nav2 packages using only rviz for visualization purposes and map_server as static map. I am not interested at the moment to moving obstacles. In order to do so I am running the following packages:

  • bt_navigator
  • planner_server
  • waypoint_follower
  • recovery_server
  • controller_server
  • map_server
  • rviz
  • static transform node (world -> map)
  • static transform node (map -> odom)

Moreover I run a custom node that replace the amcl node that is not suited for my scope. The following node subscribe the cmd_vel topic and publish each time as consequence the /odom topic and the transform odom -> base_link. The lifecyle node are all started with the following command line

ros2 run nav2_util lifecycle_bringup *nodename*

When I am setting a goal position through rviz it shows the global path correctly but the controller does not work properly.

The parameters of controller server and costmaps are:

controller_server:
  ros__parameters:
    use_sim_time: True
    controller_frequency: 5.0
    publish_cost_grid_pc: true
    controller_plugin_types: ["dwb_core::DWBLocalPlanner"]
    controller_plugin_ids: ["FollowPath"]
    min_x_velocity_threshold: 0.001
    min_y_velocity_threshold: 0.0
    min_theta_velocity_threshold: 0.001
    # DWB parameters
    FollowPath.debug_trajectory_details: True
    FollowPath.min_vel_x: 0.0
    FollowPath.min_vel_y: 0.0
    FollowPath.max_vel_x: 2.26
    FollowPath.max_vel_y: 0.0
    FollowPath.max_vel_theta: 1.0
    FollowPath.min_speed_xy: 0.0
    FollowPath.max_speed_xy: 3
    FollowPath.min_speed_theta: 0.0
    # Add high threshold velocity for turtlebot 3 issue.
    # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
    FollowPath.acc_lim_x: 2.5
    FollowPath.acc_lim_y: 0.0
    FollowPath.acc_lim_theta: 3.2
    FollowPath.decel_lim_x: -2.5
    FollowPath.decel_lim_y: 0.0
    FollowPath.decel_lim_theta: -3.2
    FollowPath.vx_samples: 20
    FollowPath.vy_samples: 5
    FollowPath.vtheta_samples: 20
    FollowPath.sim_time: 1.7
    FollowPath.linear_granularity: 0.05
    FollowPath.angular_granularity: 0.025
    FollowPath.transform_tolerance: 0.2
    FollowPath.xy_goal_tolerance: 0.25
    FollowPath.critics: ["GoalDist", "RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist"]
    FollowPath.BaseObstacle.scale: 0.02
    FollowPath.PathAlign.scale: 0.0
    FollowPath.GoalAlign.scale: 0.0
    FollowPath.PathDist.scale: 32.0
    FollowPath.GoalDist.scale: 24.0
    FollowPath.RotateToGoal.scale: 32.0
    FollowPath.short_circuit_trajectory_evaluation: True
    FollowPath.trans_stopped_velocity: 0.25
    FollowPath.slowing_factor: 5.0
    FollowPath.lookahead_time: -1.0
    FollowPath.stateful: True

controller_server_rclcpp_node:
  ros__parameters:
    use_sim_time: True
# track_unknown_space: False
local_costmap:
  local_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 5.0
      map_topic: /map
      observation_sources: ''
      lethal_cost_threshold: 254
      robot_base_frame: base_link
      use_sim_time: True
      global_frame: map 
      rolling_window: True
      width: 1
      height: 1
      resolution: 0.05  #"obstacle_layer", "voxel_layer",   "nav2_costmap_2d::ObstacleLayer", "nav2_costmap_2d::VoxelLayer",
      plugin_names: ["inflation_layer"] 
      plugin_types: ["nav2_costmap_2d::InflationLayer"]
      robot_radius: 0.2
      inflation_layer:
        cost_scaling_factor: 0.8
      obstacle_layer:
        enabled: False
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        enabled: False
        publish_voxel_map: True
        origin_z: 0.0
        z_resolution: 0.05
        z_voxels: 16
        max_obstacle_height: 2.0
        mark_threshold: 0
        observation_sources: pointcloud
        pointcloud:
          topic: /intel_realsense_r200_depth/points
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "PointCloud2"
      static_layer:
        map_subscribe_transient_local: False
      always_send_full_costmap: True
  local_costmap_client:
    ros__parameters:
      use_sim_time: True
  local_costmap_rclcpp_node:
    ros__parameters:
      use_sim_time: True

global_costmap:
  global_costmap:
    ros__parameters:
      update_frequency: 1.0
      publish_frequency: 1.0
      robot_base_frame: base_link
      global_frame: map
      map_topic: /map
      use_sim_time: True
      plugin_names: ["static_layer", "inflation_layer"]
      plugin_types: ["nav2_costmap_2d::StaticLayer", "nav2_costmap_2d::InflationLayer"]
      robot_radius: 0.2
      resolution: 0.05
      obstacle_layer:
        enabled: False
        observation_sources: scan
        scan:
          topic: /scan
          max_obstacle_height: 2.0
          clearing: True
          marking: True
          data_type: "LaserScan"
      voxel_layer:
        enabled: False
        publish_voxel_map: True ...
(more)
edit retag flag offensive close merge delete