init

ff_examples_ros2/CHANGELOG.rst

@@ -0,0 +1,11 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package ff_examples_ros2
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+Forthcoming
+-----------
+
+1.0.0 (2020-06-11)
+------------------
+* Provides examples of running free fleet servers with the ROS 1 turtlebot3 simulation examples.
+* Contributors: Aaron Chong

ff_examples_ros2/CMakeLists.txt

@@ -0,0 +1,29 @@
+cmake_minimum_required(VERSION 3.5.0)
+
+project(ff_examples_ros2)
+
+find_package(ament_cmake QUIET)
+
+if (ament_cmake_FOUND)
+
+  install(
+    DIRECTORY
+      launch
+      maps
+      params
+    DESTINATION share/${PROJECT_NAME}
+  )
+
+  install(
+    PROGRAMS 
+      scripts/send_destination_request.py
+      scripts/send_path_request.py
+      scripts/send_mode_request.py
+    DESTINATION lib/${PROJECT_NAME}
+  )
+
+  ament_package()
+
+else()
+  message("ament_cmake not found so skipping this ROS2 package")
+endif()

ff_examples_ros2/launch/fake_client.launch.xml

@@ -0,0 +1,21 @@
+<?xml version='1.0' ?>
+
+<launch>
+  <node pkg="free_fleet_client_ros2" exec="fake_action_server" name="fake_action_server" />
+
+  <node pkg="free_fleet_client_ros2" exec="fake_docking_server" name="fake_docking_server" />
+
+  <node pkg="tf2_ros" exec="static_transform_publisher" name="fake_robot_transform" args="0.0 0.0 0.0 0.0 0.0 0.0 1.0 base_footprint map" output="both"/>
+
+  <node pkg="free_fleet_client_ros2" exec="free_fleet_client_ros2" name="fake_client_node" output="both">
+    <param name="fleet_name" value="fake_fleet"/>
+    <param name="robot_name" value="fake_ros2_robot"/>
+    <param name="robot_model" value="fake_robot_model"/>
+    <param name="level_name" value="L1"/>
+    <param name="dds_domain" value="42"/>
+    <param name="max_dist_to_first_waypoint" value="10.0"/>
+    <param name="nav2_server_name" value="/navigate_to_pose_fake"/>
+    <param name="docking_trigger_server_name" value="/dock_fake"/>
+  </node>
+
+</launch>

ff_examples_ros2/launch/fake_server.launch.xml

@@ -0,0 +1,34 @@
+<?xml version='1.0' ?>
+
+<launch>
+
+  <node pkg="free_fleet_server_ros2"
+      exec="free_fleet_server_ros2"
+      name="fake_server_node"
+      output="both">
+
+    <param name="fleet_name" value="fake_fleet"/>
+
+    <param name="fleet_state_topic" value="fleet_states"/>
+    <param name="mode_request_topic" value="robot_mode_requests"/>
+    <param name="path_request_topic" value="robot_path_requests"/>
+    <param name="destination_request_topic" value="robot_destination_requests"/>
+
+    <param name="dds_domain" value="42"/>
+    <param name="dds_robot_state_topic" value="robot_state"/>
+    <param name="dds_mode_request_topic" value="mode_request"/>
+    <param name="dds_path_request_topic" value="path_request"/>
+    <param name="dds_destination_request_topic" value="destination_request"/>
+
+    <param name="update_state_frequency" value="20.0"/>
+    <param name="publish_state_frequency" value="2.0"/>
+
+    <param name="translation_x" value="-4.117"/>
+    <param name="translation_y" value="27.26"/>
+    <param name="rotation" value="-0.013"/>
+    <param name="scale" value="0.928"/>
+
+  </node>
+
+</launch>
+

ff_examples_ros2/launch/server.launch

@@ -0,0 +1,33 @@
+<?xml version='1.0' ?>
+
+<launch>
+
+  <node pkg="free_fleet_server_ros2"
+      exec="free_fleet_server_ros2"
+      name="turtlebot4_fleet_server_node"
+      output="both">
+
+    <param name="fleet_name" value="leobots"/>
+
+    <param name="fleet_state_topic" value="fleet_states"/>
+    <param name="mode_request_topic" value="robot_mode_requests"/>
+    <param name="path_request_topic" value="robot_path_requests"/>
+    <param name="destination_request_topic" value="robot_destination_requests"/>
+
+    <param name="dds_domain" value="42"/>
+    <param name="dds_robot_state_topic" value="robot_state"/>
+    <param name="dds_mode_request_topic" value="mode_request"/>
+    <param name="dds_path_request_topic" value="path_request"/>
+    <param name="dds_destination_request_topic" value="destination_request"/>
+
+    <param name="update_state_frequency" value="20.0"/>
+    <param name="publish_state_frequency" value="2.0"/>
+
+    <param name="translation_x" value="-0.22"/>
+    <param name="translation_y" value="2.28"/>
+    <param name="rotation" value="0.07"/>
+    <param name="scale" value="1.0"/>
+
+  </node>
+
+</launch>

ff_examples_ros2/launch/turtlebot3_world_ff.launch.xml

@@ -0,0 +1,33 @@
+<?xml version='1.0' ?>
+<launch>
+  <arg name="model" default="$(env TURTLEBOT3_MODEL)"/>
+  <arg name="map_file" default="$(find-pkg-share ff_examples_ros2)/maps/world.yaml"/>
+  <arg name="param_dir" default="$(find-pkg-share ff_examples_ros2)/params"/>
+
+  <!-- launches the basic turtlebot3 world with one basic turtlebot -->
+  <include file="$(find-pkg-share turtlebot3_gazebo)/launch/turtlebot3_world.launch.py"/>
+
+  <!-- launch the navigation stack with test maps -->
+  <include file="$(find-pkg-share turtlebot3_navigation2)/launch/navigation2.launch.py">
+    <arg name="use_sim_time" value="True"/>
+    <arg name="map" value="$(var map_file)"/>
+    <arg name="params_file" value="$(var param_dir)/turtlebot3_world_$(var model).yaml"/>
+  </include>
+
+  <!-- launch the free fleet client first -->
+  <node name="turtlebot3_free_fleet_client_node" pkg="free_fleet_client_ros2"
+      exec="free_fleet_client_ros2" output="both">
+    <param name="fleet_name" value="turtlebot3"/>
+    <param name="robot_name" value="ros2_tb3_0"/>
+    <param name="robot_model" value="turtlebot3"/>
+    <param name="level_name" value="turtlebot_world"/>
+    <param name="dds_domain" value="42"/>
+    <param name="max_dist_to_first_waypoint" value="10.0"/>
+    <param name="map_frame" value="map"/>
+    <param name="robot_frame" value="base_footprint"/>
+    <param name="nav2_server_name" value="/navigate_to_pose"/>
+    <param name="use_sim_time" value="True"/>
+    <param name="dds_domain" value="42"/>
+  </node>
+</launch>
+

ff_examples_ros2/launch/turtlebot3_world_ff_server.launch.xml

@@ -0,0 +1,34 @@
+<?xml version='1.0' ?>
+
+<launch>
+
+  <node pkg="free_fleet_server_ros2"
+      exec="free_fleet_server_ros2"
+      name="turtlebot3_fleet_server_node"
+      output="both">
+
+    <param name="fleet_name" value="turtlebot3"/>
+
+    <param name="fleet_state_topic" value="fleet_states"/>
+    <param name="mode_request_topic" value="robot_mode_requests"/>
+    <param name="path_request_topic" value="robot_path_requests"/>
+    <param name="destination_request_topic" value="robot_destination_requests"/>
+
+    <param name="dds_domain" value="42"/>
+    <param name="dds_robot_state_topic" value="robot_state"/>
+    <param name="dds_mode_request_topic" value="mode_request"/>
+    <param name="dds_path_request_topic" value="path_request"/>
+    <param name="dds_destination_request_topic" value="destination_request"/>
+
+    <param name="update_state_frequency" value="20.0"/>
+    <param name="publish_state_frequency" value="2.0"/>
+
+    <param name="translation_x" value="0.0"/>
+    <param name="translation_y" value="0.0"/>
+    <param name="rotation" value="0.0"/>
+    <param name="scale" value="1.0"/>
+
+  </node>
+
+</launch>
+

ff_examples_ros2/launch/turtlebot4_world_ff.launch.xml

@@ -0,0 +1,35 @@
+<?xml version='1.0' ?>
+<launch>
+  <arg name="map_file" default="$(find-pkg-share ff_examples_ros2)/maps/nicos_labyrinth.yaml"/>
+  <arg name="param_dir" default="$(find-pkg-share turtlebot4_navigation)/config"/>
+
+  <!-- launch localization with maps -->
+  <include file="$(find-pkg-share turtlebot4_navigation)/launch/localization.launch.py">
+    <arg name="use_sim_time" value="true"/>
+    <arg name="map" value="$(var map_file)"/>
+    <arg name="params_file" value="$(var param_dir)/localization.yaml"/>
+  </include>
+
+  <!-- launch the navigation stack -->
+  <include file="$(find-pkg-share turtlebot4_navigation)/launch/nav2.launch.py">
+    <arg name="use_sim_time" value="true"/>
+    <arg name="params_file" value="$(var param_dir)/nav2.yaml"/>
+  </include>
+
+  <!-- launch the free fleet client first -->
+  <node name="turtlebot4_free_fleet_client_node" pkg="free_fleet_client_ros2"
+      exec="free_fleet_client_ros2" output="both">
+    <param name="fleet_name" value="leobots"/>
+    <param name="robot_name" value="turtlebot4"/>
+    <param name="robot_model" value="turtlebot4"/>
+    <param name="level_name" value="L1"/>
+    <param name="dds_domain" value="42"/>
+    <param name="max_dist_to_first_waypoint" value="10.0"/>
+    <param name="map_frame" value="map"/>
+    <param name="robot_frame" value="base_footprint"/>
+    <param name="nav2_server_name" value="/navigate_to_pose"/>
+    <param name="use_sim_time" value="False"/>
+    <param name="dds_domain" value="42"/>
+  </node>
+</launch>
+

ff_examples_ros2/maps/house.yaml

@@ -0,0 +1,7 @@
+image: house.pgm
+resolution: 0.050000
+origin: [-10.000000, -10.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

ff_examples_ros2/maps/nicos_labyrinth.yaml

@@ -0,0 +1,7 @@
+image: nicos_labyrinth.pgm
+mode: trinary
+resolution: 0.05
+origin: [-0.0, -0.0, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

ff_examples_ros2/maps/world.yaml

@@ -0,0 +1,6 @@
+image: world.pgm
+resolution: 0.050000
+origin: [-10.000000, -10.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

ff_examples_ros2/package.xml

@@ -0,0 +1,21 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ff_examples_ros2</name>
+  <version>1.0.0</version>
+  <description>A package containing some examples of using free fleet ROS 2 components</description>
+  <maintainer email="aaron@openrobotics.org">Aaron</maintainer>
+  <license>Apache License 2.0</license>
+
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>rmf_fleet_msgs</exec_depend>
+  <exec_depend>free_fleet_client_ros2</exec_depend>
+  <exec_depend>free_fleet_server_ros2</exec_depend>
+  <exec_depend>turtlebot3_gazebo</exec_depend>
+  <exec_depend>turtlebot3_navigation2</exec_depend>
+  <exec_depend>turtlebot4_navigation</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ff_examples_ros2/params/turtlebot3_world_burger.yaml

@@ -0,0 +1,356 @@
+amcl:
+  ros__parameters:
+    use_sim_time: False
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+    set_initial_pose: true
+    initial_pose:
+      x: -1.8949996233
+      y: -0.419999837875
+      z: 0.0
+      a: -0.0208304
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: False
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    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"]
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.22
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.22
+      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: 20
+      sim_time: 1.7
+      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: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.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: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.1
+      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 1.0
+        inflation_radius: 0.55
+      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"
+      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.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          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: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.1
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "voxel_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
+      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.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: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    yaml_filename: "map.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: False
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+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: base_link
+    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
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 2000
+    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

ff_examples_ros2/params/turtlebot3_world_waffle.yaml

@@ -0,0 +1,356 @@
+amcl:
+  ros__parameters:
+    use_sim_time: False
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    scan_topic: scan
+    set_initial_pose: true
+    initial_pose:
+      x: -1.8949996233
+      y: -0.419999837875
+      z: 0.0
+      a: -0.0208304
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: False
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    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"]
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.22
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.22
+      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: 20
+      sim_time: 1.7
+      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: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.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: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 1.0
+        inflation_radius: 0.55
+      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"
+      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.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          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: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "voxel_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
+      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.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: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    yaml_filename: "map.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: False
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+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: base_link
+    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
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 200
+    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

ff_examples_ros2/params/turtlebot3_world_waffle_pi.yaml

@@ -0,0 +1,355 @@
+amcl:
+  ros__parameters:
+    use_sim_time: False
+    alpha1: 0.2
+    alpha2: 0.2
+    alpha3: 0.2
+    alpha4: 0.2
+    alpha5: 0.2
+    base_frame_id: "base_footprint"
+    beam_skip_distance: 0.5
+    beam_skip_error_threshold: 0.9
+    beam_skip_threshold: 0.3
+    do_beamskip: false
+    global_frame_id: "map"
+    lambda_short: 0.1
+    laser_likelihood_max_dist: 2.0
+    laser_max_range: 100.0
+    laser_min_range: -1.0
+    laser_model_type: "likelihood_field"
+    max_beams: 60
+    max_particles: 2000
+    min_particles: 500
+    odom_frame_id: "odom"
+    pf_err: 0.05
+    pf_z: 0.99
+    recovery_alpha_fast: 0.0
+    recovery_alpha_slow: 0.0
+    resample_interval: 1
+    robot_model_type: "nav2_amcl::DifferentialMotionModel"
+    save_pose_rate: 0.5
+    sigma_hit: 0.2
+    tf_broadcast: true
+    transform_tolerance: 1.0
+    update_min_a: 0.2
+    update_min_d: 0.25
+    z_hit: 0.5
+    z_max: 0.05
+    z_rand: 0.5
+    z_short: 0.05
+    set_initial_pose: true
+    initial_pose:
+      x: -1.8949996233
+      y: -0.419999837875
+      z: 0.0
+      a: -0.0208304
+
+amcl_map_client:
+  ros__parameters:
+    use_sim_time: False
+
+amcl_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+bt_navigator:
+  ros__parameters:
+    use_sim_time: False
+    global_frame: map
+    robot_base_frame: base_link
+    odom_topic: /odom
+    default_bt_xml_filename: "navigate_w_replanning_and_recovery.xml"
+    bt_loop_duration: 10
+    default_server_timeout: 20
+    enable_groot_monitoring: True
+    groot_zmq_publisher_port: 1666
+    groot_zmq_server_port: 1667
+    plugin_lib_names:
+    - nav2_compute_path_to_pose_action_bt_node
+    - nav2_compute_path_through_poses_action_bt_node
+    - nav2_follow_path_action_bt_node
+    - nav2_back_up_action_bt_node
+    - nav2_spin_action_bt_node
+    - nav2_wait_action_bt_node
+    - nav2_clear_costmap_service_bt_node
+    - nav2_is_stuck_condition_bt_node
+    - nav2_goal_reached_condition_bt_node
+    - nav2_goal_updated_condition_bt_node
+    - nav2_initial_pose_received_condition_bt_node
+    - nav2_reinitialize_global_localization_service_bt_node
+    - nav2_rate_controller_bt_node
+    - nav2_distance_controller_bt_node
+    - nav2_speed_controller_bt_node
+    - nav2_truncate_path_action_bt_node
+    - nav2_goal_updater_node_bt_node
+    - nav2_recovery_node_bt_node
+    - nav2_pipeline_sequence_bt_node
+    - nav2_round_robin_node_bt_node
+    - nav2_transform_available_condition_bt_node
+    - nav2_time_expired_condition_bt_node
+    - nav2_distance_traveled_condition_bt_node
+    - nav2_single_trigger_bt_node
+    - nav2_is_battery_low_condition_bt_node
+    - nav2_navigate_through_poses_action_bt_node
+    - nav2_navigate_to_pose_action_bt_node
+    - nav2_remove_passed_goals_action_bt_node
+    - nav2_planner_selector_bt_node
+    - nav2_controller_selector_bt_node
+    - nav2_goal_checker_selector_bt_node
+
+bt_navigator_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+controller_server:
+  ros__parameters:
+    use_sim_time: False
+    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"]
+    controller_plugins: ["FollowPath"]
+
+    # Progress checker parameters
+    progress_checker:
+      plugin: "nav2_controller::SimpleProgressChecker"
+      required_movement_radius: 0.5
+      movement_time_allowance: 10.0
+
+    general_goal_checker:
+      stateful: True
+      plugin: "nav2_controller::SimpleGoalChecker"
+      xy_goal_tolerance: 0.25
+      yaw_goal_tolerance: 0.25
+
+    # DWB parameters
+    FollowPath:
+      plugin: "dwb_core::DWBLocalPlanner"
+      debug_trajectory_details: True
+      min_vel_x: 0.0
+      min_vel_y: 0.0
+      max_vel_x: 0.22
+      max_vel_y: 0.0
+      max_vel_theta: 1.0
+      min_speed_xy: 0.0
+      max_speed_xy: 0.22
+      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: 20
+      sim_time: 1.7
+      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: 32.0
+      PathAlign.forward_point_distance: 0.1
+      GoalAlign.scale: 24.0
+      GoalAlign.forward_point_distance: 0.1
+      PathDist.scale: 32.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: False
+
+local_costmap:
+  local_costmap:
+    ros__parameters:
+      update_frequency: 5.0
+      publish_frequency: 2.0
+      global_frame: odom
+      robot_base_frame: base_link
+      use_sim_time: False
+      rolling_window: true
+      width: 3
+      height: 3
+      resolution: 0.05
+      robot_radius: 0.22
+      plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
+      inflation_layer:
+        plugin: "nav2_costmap_2d::InflationLayer"
+        cost_scaling_factor: 1.0
+        inflation_radius: 0.55
+      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"
+      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.0
+          raytrace_min_range: 0.0
+          obstacle_max_range: 2.5
+          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: False
+  local_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+global_costmap:
+  global_costmap:
+    ros__parameters:
+      update_frequency: 1.0
+      publish_frequency: 1.0
+      global_frame: map
+      robot_base_frame: base_link
+      use_sim_time: True
+      robot_radius: 0.22
+      resolution: 0.05
+      track_unknown_space: true
+      plugins: ["static_layer", "obstacle_layer", "voxel_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
+      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.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: 3.0
+        inflation_radius: 0.55
+      always_send_full_costmap: True
+  global_costmap_client:
+    ros__parameters:
+      use_sim_time: False
+  global_costmap_rclcpp_node:
+    ros__parameters:
+      use_sim_time: False
+
+map_server:
+  ros__parameters:
+    use_sim_time: False
+    yaml_filename: "map.yaml"
+
+map_saver:
+  ros__parameters:
+    use_sim_time: False
+    save_map_timeout: 5.0
+    free_thresh_default: 0.25
+    occupied_thresh_default: 0.65
+    map_subscribe_transient_local: True
+
+planner_server:
+  ros__parameters:
+    expected_planner_frequency: 20.0
+    use_sim_time: False
+    planner_plugins: ["GridBased"]
+    GridBased:
+      plugin: "nav2_navfn_planner/NavfnPlanner"
+      tolerance: 0.5
+      use_astar: false
+      allow_unknown: true
+
+planner_server_rclcpp_node:
+  ros__parameters:
+    use_sim_time: False
+
+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: base_link
+    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
+
+robot_state_publisher:
+  ros__parameters:
+    use_sim_time: False
+
+waypoint_follower:
+  ros__parameters:
+    loop_rate: 200
+    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

ff_examples_ros2/scripts/send_destination_request.py

@@ -0,0 +1,89 @@
+#!/usr/bin/env python3
+
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import sys
+import argparse
+
+import rclpy
+from rclpy.node import Node
+
+from rmf_fleet_msgs.msg import Location
+from rmf_fleet_msgs.msg import DestinationRequest
+
+
+def main(argv = sys.argv):
+    '''
+    Example destination request:
+    - fleet_name: magni
+    - robot_name: magni123
+    - task_id: 6tyghb4edujrefyd
+    - x: 0.0
+    - y: 0.0
+    - yaw: 0.0
+    - level_name: B1
+    '''
+
+    default_fleet_name = 'fleet_name'
+    default_robot_name = 'robot_name'
+    default_task_id = '6tyghb4edujrefyd'
+    default_desired_x = 0.0
+    default_desired_y = 0.0
+    default_desired_yaw = 0.0
+    default_level_name = 'B1'
+    default_topic_name = 'robot_destination_requests'
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument('-f', '--fleet-name', default=default_fleet_name)
+    parser.add_argument('-r', '--robot-name', default=default_robot_name)
+    parser.add_argument('-x', default=default_desired_x)
+    parser.add_argument('-y', default=default_desired_y)
+    parser.add_argument('--yaw', default=default_desired_yaw)
+    parser.add_argument('-l', '--level-name', default=default_level_name)
+    parser.add_argument('-i', '--task-id', default=default_task_id)
+    parser.add_argument('-t', '--topic-name', default=default_topic_name)
+    args = parser.parse_args(argv[1:])
+
+    print('fleet_name: {}'.format(args.fleet_name))
+    print('robot_name: {}'.format(args.robot_name))
+    print('x: {}'.format(args.x))
+    print('y: {}'.format(args.y))
+    print('yaw: {}'.format(args.yaw))
+    print('level_name: {}'.format(args.level_name))
+    print('task_id: {}'.format(args.task_id))
+    print('topic_name: {}'.format(args.topic_name))
+
+    rclpy.init()
+    node = rclpy.create_node('send_destination_request_node')
+    pub = node.create_publisher(DestinationRequest, args.topic_name, 10)
+
+    msg = DestinationRequest()
+    msg.fleet_name = args.fleet_name
+    msg.robot_name = args.robot_name
+    msg.task_id = args.task_id
+    # ignore time for now
+    msg.destination.x = float(args.x)
+    msg.destination.y = float(args.y)
+    msg.destination.yaw = float(args.yaw)
+    msg.destination.level_name = args.level_name
+
+    rclpy.spin_once(node, timeout_sec=1.0)
+    pub.publish(msg)
+    rclpy.spin_once(node, timeout_sec=0.5)
+    print('all done!')
+
+
+if __name__ == '__main__':
+    main(sys.argv)

ff_examples_ros2/scripts/send_mode_request.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import sys
+import argparse
+
+import rclpy
+from rclpy.node import Node
+
+from rmf_fleet_msgs.msg import RobotMode
+from rmf_fleet_msgs.msg import ModeRequest
+
+
+def main(argv = sys.argv):
+    '''
+    Example mode request:
+    - fleet_name: magni
+    - robot_name: magni123
+    - task_id: 576y13ewgyffeijuais
+    - mode.mode: PAUSED
+    '''
+
+    default_fleet_name = 'fleet_name'
+    default_robot_name = 'robot_name'
+    default_task_id = '576y13ewgyffeijuais'
+    default_mode = 'mode'
+    default_topic_name = 'robot_mode_requests'
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument('-f', '--fleet-name', default=default_fleet_name)
+    parser.add_argument('-r', '--robot-name', default=default_robot_name)
+    parser.add_argument('-m', '--mode', default=default_mode)
+    parser.add_argument('-i', '--task-id', default=default_task_id)
+    parser.add_argument('-t', '--topic-name', default=default_topic_name)
+    args = parser.parse_args(argv[1:])
+
+    print('fleet_name: {}'.format(args.fleet_name))
+    print('robot_name: {}'.format(args.robot_name))
+    print('mode: {}'.format(args.mode))
+    print('task_id: {}'.format(args.task_id))
+    print('topic_name: {}'.format(args.topic_name))
+
+    rclpy.init()
+    node = rclpy.create_node('send_mode_request_node')
+    pub = node.create_publisher(ModeRequest, args.topic_name, 10)
+
+    msg = ModeRequest()
+    msg.fleet_name = args.fleet_name
+    msg.robot_name = args.robot_name
+    msg.task_id = args.task_id
+    
+    if args.mode == 'mode':
+        print('Please insert desired mode, pause or resume')
+        return
+    elif args.mode == 'pause':
+        msg.mode.mode = RobotMode.MODE_PAUSED
+    elif args.mode == 'resume':
+        msg.mode.mode = RobotMode.MODE_MOVING
+    else:
+        print('unrecognized mode requested, only use pause or resume please')
+        return
+  
+    rclpy.spin_once(node, timeout_sec=1.0)
+    pub.publish(msg)
+    rclpy.spin_once(node, timeout_sec=0.5)
+    print('all done!')
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main(sys.argv)

ff_examples_ros2/scripts/send_path_request.py

@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+ 
+# Copyright 2019 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import sys
+import json
+import argparse
+
+import rclpy
+from rclpy.node import Node
+
+from rmf_fleet_msgs.msg import Location
+from rmf_fleet_msgs.msg import PathRequest
+
+
+def print_path(json_path_string):
+    path_dict = json.loads(json_path_string)
+    for waypoint in path_dict:
+        print('01 x: {} y: {} yaw: {} level_name: {}'.format(
+                waypoint['x'], waypoint['y'], waypoint['yaw'],
+                waypoint['level_name']))
+
+
+def get_path_message(json_path_string):
+    path_dict = json.loads(json_path_string)
+    path = []
+    for waypoint in path_dict:
+        new_wp = Location()
+        # ignore time for now
+        new_wp.x = float(waypoint['x'])
+        new_wp.y = float(waypoint['y'])
+        new_wp.yaw = float(waypoint['yaw'])
+        new_wp.level_name = waypoint['level_name']
+        path.append(new_wp)
+    return path
+
+
+def main(argv = sys.argv):
+    '''
+    Example path request:
+    - fleet_name: magni
+    - robot_name: magni123
+    - task_id: tfuhbjndsujewsduisd
+    - path: [
+        {"x": 0.0, "y": 0.0, "yaw": 0.0, "level_name": "B1"},
+        {"x": 1.0, "y": 1.0, "yaw": 1.0, "level_name": "B1"},
+        {"x": 2.0, "y": 2.0, "yaw": 2.0, "level_name": "B1"}]
+    '''
+
+    default_fleet_name = 'fleet_name'
+    default_robot_name = 'robot_name'
+    default_task_id = 'tfuhbjndsujewsduisd'
+    default_desired_path = '[]'
+    default_topic_name = 'robot_path_requests'
+
+    parser = argparse.ArgumentParser()
+    parser.add_argument('-f', '--fleet-name', default=default_fleet_name)
+    parser.add_argument('-r', '--robot-name', default=default_robot_name)
+    parser.add_argument('-i', '--task-id', default=default_task_id)
+    parser.add_argument('-p', '--path', default=default_desired_path)
+    parser.add_argument('-t', '--topic-name', default=default_topic_name)
+    args = parser.parse_args(argv[1:])
+
+    print('fleet_name: {}'.format(args.fleet_name))
+    print('robot_name: {}'.format(args.robot_name))
+    print('task_id: {}'.format(args.task_id))
+    print_path(args.path)
+    print('topic_name: {}'.format(args.topic_name))
+
+    rclpy.init()
+    node = rclpy.create_node('send_path_request_node')
+    pub = node.create_publisher(PathRequest, args.topic_name, 10)
+
+    msg = PathRequest()
+    msg.fleet_name = args.fleet_name
+    msg.robot_name = args.robot_name
+    msg.task_id = args.task_id
+    msg.path = get_path_message(args.path)
+
+    rclpy.spin_once(node, timeout_sec=2.0)
+    pub.publish(msg)
+    rclpy.spin_once(node, timeout_sec=0.5)
+    print('all done!')
+
+
+if __name__ == '__main__':
+    main(sys.argv)

free_fleet

@@ -1 +0,0 @@
-Subproject commit d385372b9b9973abef35986c1a2175238ff5bf09

rmf_internal_msgs

@@ -1 +0,0 @@
-Subproject commit 944a6f4f37c9649234c243e44f168b30e0678c7c