Merge pull request IMRCLab#81 from IMRCLab/nav2_tutorial

Tutorial with nav2 bringup and crazyflie with multiranger

crazyflie/scripts/crazyflie_server.py

@@ -87,6 +87,11 @@ def __init__(self):
  "scan": self._log_scan_data_callback,
  "odom": self._log_odom_data_callback}
 
+ self.world_tf_name = "world"
+ try:
+ self.world_tf_name = self._ros_parameters["world_tf_name"]
+ except KeyError:
+ pass
  robot_data = self._ros_parameters["robots"]
 
  # Init a transform broadcaster
@@ -453,7 +458,7 @@ def _log_pose_data_callback(self, timestamp, data, logconf, uri):
 
  msg = PoseStamped()
  msg.header.stamp = self.get_clock().now().to_msg()
- msg.header.frame_id = "world"
+ msg.header.frame_id = self.world_tf_name
  msg.pose.position.x = x
  msg.pose.position.y = y
  msg.pose.position.z = z
@@ -465,7 +470,7 @@ def _log_pose_data_callback(self, timestamp, data, logconf, uri):
 
  t_base = TransformStamped()
  t_base.header.stamp = self.get_clock().now().to_msg()
- t_base.header.frame_id = 'world'
+ t_base.header.frame_id = self.world_tf_name
  t_base.child_frame_id = cf_name
  t_base.transform.translation.x = x
  t_base.transform.translation.y = y
@@ -493,7 +498,7 @@ def _log_odom_data_callback(self, timestamp, data, logconf, uri):
  msg = Odometry()
  msg.child_frame_id = cf_name
  msg.header.stamp = self.get_clock().now().to_msg()
- msg.header.frame_id = "world"
+ msg.header.frame_id = self.world_tf_name
  msg.pose.pose.position.x = x
  msg.pose.pose.position.y = y
  msg.pose.pose.orientation.x = q[0]

crazyflie_examples/config/nav2_params.yaml

@@ -0,0 +1,229 @@
+controller_server:
+ ros__parameters:
+ use_sim_time: True
+ controller_frequency: 20.0
+ min_x_velocity_threshold: 0.001
+ min_y_velocity_threshold: 0.5
+ min_theta_velocity_threshold: 0.001
+ failure_tolerance: 0.3
+ progress_checker_plugin: "progress_checker"
+ goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker"
+ controller_plugins: ["FollowPath"]
+
+ # Progress checker parameters
+ progress_checker:
+ plugin: "nav2_controller::SimpleProgressChecker"
+ required_movement_radius: 0.5
+ movement_time_allowance: 10.0
+ # Goal checker parameters
+ #precise_goal_checker:
+ # plugin: "nav2_controller::SimpleGoalChecker"
+ # xy_goal_tolerance: 0.25
+ # yaw_goal_tolerance: 0.25
+ # stateful: True
+ general_goal_checker:
+ stateful: True
+ plugin: "nav2_controller::SimpleGoalChecker"
+ xy_goal_tolerance: 0.25
+ yaw_goal_tolerance: 0.25
+ # FollowPath:
+ # plugin: "nav2_regulated_pure_pursuit_controller::RegulatedPurePursuitController"
+ # desired_linear_vel: 0.5
+ # lookahead_dist: 0.6
+ # min_lookahead_dist: 0.3
+ # max_lookahead_dist: 0.9
+ # lookahead_time: 1.5
+ # rotate_to_heading_angular_vel: 1.8
+ # transform_tolerance: 0.1
+ # use_velocity_scaled_lookahead_dist: false
+ # min_approach_linear_velocity: 0.05
+ # max_allowed_time_to_collision_up_to_carrot: 1.0
+ # use_regulated_linear_velocity_scaling: true
+ # use_cost_regulated_linear_velocity_scaling: true
+ # regulated_linear_scaling_min_radius: 0.9
+ # regulated_linear_scaling_min_speed: 0.25
+ # use_rotate_to_heading: false
+ # allow_reversing: true
+ # rotate_to_heading_min_angle: 0.2
+ # max_angular_accel: 3.2
+ # max_robot_pose_search_dist: 10.0
+ # DWB parameters
+ FollowPath:
+ plugin: "dwb_core::DWBLocalPlanner"
+ debug_trajectory_details: True
+ min_vel_x: -0.26
+ min_vel_y: 0.0
+ max_vel_x: 0.26
+ max_vel_y: 0.0
+ max_vel_theta: 1.0
+ min_speed_xy: 0.0
+ max_speed_xy: 0.26
+ min_speed_theta: 0.0
+ # Add high threshold velocity for turtlebot 3 issue.
+ # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
+ acc_lim_x: 2.5
+ acc_lim_y: 0.0
+ acc_lim_theta: 3.2
+ decel_lim_x: -2.5
+ decel_lim_y: 0.0
+ decel_lim_theta: -3.2
+ vx_samples: 20
+ vy_samples: 5
+ vtheta_samples: 40
+ sim_time: 3.0
+ linear_granularity: 0.05
+ angular_granularity: 0.025
+ transform_tolerance: 0.2
+ xy_goal_tolerance: 0.25
+ trans_stopped_velocity: 0.25
+ short_circuit_trajectory_evaluation: True
+ stateful: True
+ critics: ["RotateToGoal", "Oscillation", "BaseObstacle", "GoalAlign", "PathAlign", "PathDist", "GoalDist"]
+ BaseObstacle.scale: 0.02
+ PathAlign.scale: 80.0
+ PathAlign.forward_point_distance: 0.1
+ GoalAlign.scale: 24.0
+ GoalAlign.forward_point_distance: 0.1
+ PathDist.scale: 80.0
+ GoalDist.scale: 24.0
+ RotateToGoal.scale: 32.0
+ RotateToGoal.slowing_factor: 5.0
+ RotateToGoal.lookahead_time: -1.0
+
+
+controller_server_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+local_costmap:
+ local_costmap:
+ ros__parameters:
+ update_frequency: 5.0
+ publish_frequency: 2.0
+ global_frame: odom
+ robot_base_frame: cf1
+ use_sim_time: true
+ rolling_window: true
+ width: 10
+ height: 10
+ resolution: 0.05
+ robot_radius: 0.25
+ plugins: ["voxel_layer", "inflation_layer"]
+ inflation_layer:
+ plugin: "nav2_costmap_2d::InflationLayer"
+ cost_scaling_factor: 1.0
+ inflation_radius: 0.3
+ voxel_layer:
+ plugin: "nav2_costmap_2d::VoxelLayer"
+ enabled: True
+ 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: scan
+ scan:
+ topic: /scan
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "LaserScan"
+ raytrace_max_range: 3.5
+ raytrace_min_range: 0.0
+ obstacle_max_range: 3.2
+ obstacle_min_range: 0.0
+ static_layer:
+ map_subscribe_transient_local: True
+ 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
+ global_frame: map
+ robot_base_frame: cf1
+ use_sim_time: True
+ robot_radius: 0.25
+ resolution: 0.05
+ track_unknown_space: true
+ plugins: ["static_layer", "obstacle_layer", "inflation_layer"]
+ obstacle_layer:
+ plugin: "nav2_costmap_2d::ObstacleLayer"
+ enabled: True
+ observation_sources: scan
+ scan:
+ topic: /scan
+ max_obstacle_height: 2.0
+ clearing: True
+ marking: True
+ data_type: "LaserScan"
+ raytrace_max_range: 3.0
+ raytrace_min_range: 0.0
+ obstacle_max_range: 2.5
+ obstacle_min_range: 0.0
+ static_layer:
+ plugin: "nav2_costmap_2d::StaticLayer"
+ map_subscribe_transient_local: True
+ inflation_layer:
+ plugin: "nav2_costmap_2d::InflationLayer"
+ cost_scaling_factor: 1.0
+ inflation_radius: 0.3
+ always_send_full_costmap: True
+ global_costmap_client:
+ ros__parameters:
+ use_sim_time: True
+ global_costmap_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+planner_server:
+ ros__parameters:
+ planner_plugins: ['GridBased']
+ GridBased:
+ plugin: 'nav2_navfn_planner/NavfnPlanner'
+ use_astar: True
+ allow_unknown: True
+ tolerance: 1.0
+
+planner_server_rclcpp_node:
+ ros__parameters:
+ use_sim_time: True
+
+recoveries_server:
+ ros__parameters:
+ costmap_topic: local_costmap/costmap_raw
+ footprint_topic: local_costmap/published_footprint
+ cycle_frequency: 10.0
+ recovery_plugins: ["spin", "backup", "wait"]
+ spin:
+ plugin: "nav2_recoveries/Spin"
+ backup:
+ plugin: "nav2_recoveries/BackUp"
+ wait:
+ plugin: "nav2_recoveries/Wait"
+ global_frame: odom
+ robot_base_frame: cf1
+ transform_timeout: 0.1
+ use_sim_time: true
+ simulate_ahead_time: 2.0
+ max_rotational_vel: 1.0
+ min_rotational_vel: 0.4
+ rotational_acc_lim: 3.2
+
+waypoint_follower:
+ ros__parameters:
+ loop_rate: 20
+ stop_on_failure: false
+ waypoint_task_executor_plugin: "wait_at_waypoint" 
+ wait_at_waypoint:
+ plugin: "nav2_waypoint_follower::WaitAtWaypoint"
+ enabled: True
+ waypoint_pause_duration: 200