Example with rmf_demos commit 2371649

Signed-off-by: chianfern <chianfern@gmail.com>

README.md

@@ -1 +1,96 @@
 # Free Fleet ROS2
+
+## Contents
+
+- **[About](#About)**
+- **[Installation Instructions](#installation-instructions)**
+  - [Prerequisites](#prerequisites)
+- **[Examples](#examples)**
+</br>
+</br>
+
+## About
+
+Implementation of a Fleet Adapter to integrate [Free Fleet (an upcoming version)](https://github.com/open-rmf/free_fleet/tree/develop) clients with RMF [demos](https://github.com/open-rmf/rmf_demos).
+
+</br>
+</br>
+
+## Installation Instructions
+
+### Prerequisites
+
+* [Ubuntu 20.04 LTS](https://releases.ubuntu.com/20.04/)
+* [ROS2 - Galactic](https://docs.ros.org/en/galactic/index.html)
+* [RMF demos](https://github.com/open-rmf/rmf_demos)
+* [Free Fleet - develop branch](https://github.com/open-rmf/free_fleet/tree/develop)
+
+</br>
+
+### Installation
+Assuming free_fleet and fleet_fleet_ros2 are to be located in your home directory.
+
+#### Free Fleet (develop)
+
+```
+cd ~
+git clone https://github.com/open-rmf/free_fleet.git -b develop
+source /opt/ros/galactic/setup.bash
+source ~/rmf_ws/install/setup.bash
+cd ~/free_fleet
+# free_fleet dependencies
+colcon build
+```
+
+#### Free Fleet ROS2 Implementation
+```bash
+cd ~
+git clone https://github.com/open-rmf/free_fleet_ros2.git -b develop
+source /opt/ros/galactic/setup.bash
+source ~/rmf_ws/install/setup.bash # requires rmf_fleet_msgs and rmf_task_msgs
+source ~/free_fleet/install/setup.bash
+colcon build
+```
+### Examples
+
+To get the demos to use the free_fleet_ros2 adapter instead, modify this launch file:
+Edit ~/rmf_ws/src/rmf/rmf_ros2/rmf_fleet_adapter/launch/fleet_adapter.launch.xml
+Comment out lines 43-46
+```xml
+  <!-- <node pkg="rmf_fleet_adapter"
+        exec="$(var control_type)"
+        name="$(var fleet_name)_fleet_adapter"
+        output="both"> -->
+  <node pkg="free_fleet_ros2_adapter"
+        exec="adapter"
+        name="free_fleet_ros2_adapter"
+        output="both">
+```
+
+Rebuild to use the updated launch file:
+`cd ~/rmf_ws; colcon build --packages-select rmf_fleet_adapter`
+
+Open one terminal for the demo+adapters and one for the clients.
+In each:
+```
+source /opt/ros/galactic/setup.bash
+source ~/rmf_ws/install/setup.bash
+source ~/free_fleet_ros2/install/setup.bash
+
+```
+#### Launch demo
+```
+ros2 launch rmf_demos_gz hotel.launch.xml
+```
+#### Launch clients
+The demo should be running before the clients are started.
+```
+ros2 launch free_fleet_ros2_examples hotel_clients.launch.xml
+```
+
+### To do
+- During Cleaning and other tasks which are performed while docking, visualization will not work correctly.
+- Support new speed limit in Location.
+- Complete testing of opening and closing of lanes.
+- Better approach to launch files and address upcoming changes to `rmf_demos` launch and config files.
+- Launch files for remaining demo worlds.

adapter/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.7.0)
+project(free_fleet_ros2_adapter)
+
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 17)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+if(NOT CMAKE_BUILD_TYPE)
+  # Use the Release build type by default if the user has not specified one
+  set(CMAKE_BUILD_TYPE Release)
+endif()
+
+find_package(ament_cmake REQUIRED)
+include(GNUInstallDirs)
+
+set(dep_pkgs
+  rclcpp
+  rmf_fleet_msgs
+  rmf_task_msgs
+  rmf_traffic
+  rmf_traffic_ros2
+  rmf_fleet_adapter
+  free_fleet
+  free_fleet_cyclonedds
+  CycloneDDS
+)
+foreach(pkg ${dep_pkgs})
+  find_package(${pkg} REQUIRED)
+endforeach()
+
+# -----------------------------------------------------------------------------
+add_executable(adapter 
+  src/main.cpp
+)
+
+target_link_libraries(adapter
+  PRIVATE
+    free_fleet::free_fleet
+    free_fleet_cyclonedds::free_fleet_cyclonedds
+    rmf_fleet_adapter::rmf_fleet_adapter
+    rmf_traffic::rmf_traffic
+    ${rclcpp_LIBRARIES}
+    ${rmf_fleet_msgs_LIBRARIES}
+    ${rmf_task_msgs_LIBRARIES}
+    CycloneDDS::ddsc
+)
+target_include_directories(adapter
+  PRIVATE
+    ${rmf_fleet_msgs_INCLUDE_DIRS}
+    ${rmf_task_msgs_INCLUDE_DIRS}
+)
+
+install(TARGETS
+  adapter
+  DESTINATION lib/${PROJECT_NAME}
+)
+ament_package()

adapter/package.xml

@@ -0,0 +1,28 @@
+<?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>free_fleet_ros2_adapter</name>
+  <version>1.1.0</version>
+  <description>Free fleet ROS 2 implementations</description>
+  <maintainer email="aaron@openrobotics.org">Aaron</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>rclcpp</depend>
+  <depend>rmf_utils</depend>
+  <depend>rmf_traffic</depend>
+  <depend>rmf_traffic_ros2</depend>
+  <depend>rmf_fleet_adapter</depend>
+
+  <depend>free_fleet</depend>
+  <depend>free_fleet_cyclonedds</depend>
+
+  <build_depend>eigen</build_depend>
+
+  <test_depend>ament_cmake_catch2</test_depend>
+  <test_depend>rmf_cmake_uncrustify</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

adapter/src/load_param.cpp

@@ -0,0 +1,133 @@
+/*
+ * Copyright (C) 2019 Open Source Robotics Foundation
+ *
+ * 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.
+ *
+*/
+
+#include "load_param.hpp"
+
+#include <rmf_traffic/geometry/Circle.hpp>
+
+namespace rmf_fleet_adapter {
+
+//==============================================================================
+std::chrono::nanoseconds get_parameter_or_default_time(
+  rclcpp::Node& node,
+  const std::string& param_name,
+  const double default_value)
+{
+  const double value =
+    get_parameter_or_default(node, param_name, default_value);
+
+  return std::chrono::duration_cast<std::chrono::nanoseconds>(
+    std::chrono::duration<double, std::ratio<1>>(value));
+}
+
+//==============================================================================
+std::string get_fleet_name_parameter(rclcpp::Node& node)
+{
+  std::string fleet_name = node.declare_parameter("fleet_name", std::string());
+  if (fleet_name.empty())
+  {
+    RCLCPP_ERROR(
+      node.get_logger(),
+      "The fleet_name parameter must be specified!");
+    throw std::runtime_error("fleet_name parameter is missing");
+  }
+
+  return fleet_name;
+}
+
+//==============================================================================
+rmf_traffic::agv::VehicleTraits get_traits_or_default(rclcpp::Node& node,
+  const double default_v_nom, const double default_w_nom,
+  const double default_a_nom, const double default_alpha_nom,
+  const double default_r_f, const double default_r_v)
+{
+  const double v_nom =
+    get_parameter_or_default(node, "linear_velocity", default_v_nom);
+  const double w_nom =
+    get_parameter_or_default(node, "angular_velocity", default_w_nom);
+  const double a_nom =
+    get_parameter_or_default(node, "linear_acceleration", default_a_nom);
+  const double b_nom =
+    get_parameter_or_default(node, "angular_acceleration", default_alpha_nom);
+  const double r_f =
+    get_parameter_or_default(node, "footprint_radius", default_r_f);
+  const double r_v =
+    get_parameter_or_default(node, "vicinity_radius", default_r_v);
+  const bool reversible =
+    get_parameter_or_default(node, "reversible", true);
+
+  if (!reversible)
+    std::cout << " ===== We have an irreversible robot" << std::endl;
+
+  auto traits = rmf_traffic::agv::VehicleTraits{
+    {v_nom, a_nom},
+    {w_nom, b_nom},
+    rmf_traffic::Profile{
+      rmf_traffic::geometry::make_final_convex<
+        rmf_traffic::geometry::Circle>(r_f),
+      rmf_traffic::geometry::make_final_convex<
+        rmf_traffic::geometry::Circle>(r_v)
+    }
+  };
+
+  traits.get_differential()->set_reversible(reversible);
+  return traits;
+}
+
+
+//==============================================================================
+std::optional<rmf_battery::agv::BatterySystem> get_battery_system(
+  rclcpp::Node& node,
+  const double default_voltage,
+  const double default_capacity,
+  const double default_charging_current)
+{
+  const double voltage =
+    get_parameter_or_default(node, "battery_voltage", default_voltage);
+  const double capacity =
+    get_parameter_or_default(node, "battery_capacity", default_capacity);
+  const double charging_current =
+    get_parameter_or_default(
+    node, "battery_charging_current", default_charging_current);
+
+  auto battery_system = rmf_battery::agv::BatterySystem::make(
+    voltage, capacity, charging_current);
+
+  return battery_system;
+}
+
+//==============================================================================
+std::optional<rmf_battery::agv::MechanicalSystem> get_mechanical_system(
+  rclcpp::Node& node,
+  const double default_mass,
+  const double default_moment,
+  const double default_friction)
+{
+  const double mass =
+    get_parameter_or_default(node, "mass", default_mass);
+  const double moment_of_inertia =
+    get_parameter_or_default(node, "inertia", default_moment);
+  const double friction =
+    get_parameter_or_default(node, "friction_coefficient", default_friction);
+
+  auto mechanical_system = rmf_battery::agv::MechanicalSystem::make(
+    mass, moment_of_inertia, friction);
+
+  return mechanical_system;
+}
+
+} // namespace rmf_fleet_adapter