|
| 1 | +/* |
| 2 | + * Copyright (C) 2019 Open Source Robotics Foundation |
| 3 | + * |
| 4 | + * Licensed under the Apache License, Version 2.0 (the "License"); |
| 5 | + * you may not use this file except in compliance with the License. |
| 6 | + * You may obtain a copy of the License at |
| 7 | + * |
| 8 | + * http://www.apache.org/licenses/LICENSE-2.0 |
| 9 | + * |
| 10 | + * Unless required by applicable law or agreed to in writing, software |
| 11 | + * distributed under the License is distributed on an "AS IS" BASIS, |
| 12 | + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 13 | + * See the License for the specific language governing permissions and |
| 14 | + * limitations under the License. |
| 15 | + * |
| 16 | +*/ |
| 17 | + |
| 18 | +#include "load_param.hpp" |
| 19 | + |
| 20 | +#include <rmf_traffic/geometry/Circle.hpp> |
| 21 | + |
| 22 | +namespace rmf_fleet_adapter { |
| 23 | + |
| 24 | +//============================================================================== |
| 25 | +std::chrono::nanoseconds get_parameter_or_default_time( |
| 26 | + rclcpp::Node& node, |
| 27 | + const std::string& param_name, |
| 28 | + const double default_value) |
| 29 | +{ |
| 30 | + const double value = |
| 31 | + get_parameter_or_default(node, param_name, default_value); |
| 32 | + |
| 33 | + return std::chrono::duration_cast<std::chrono::nanoseconds>( |
| 34 | + std::chrono::duration<double, std::ratio<1>>(value)); |
| 35 | +} |
| 36 | + |
| 37 | +//============================================================================== |
| 38 | +std::string get_fleet_name_parameter(rclcpp::Node& node) |
| 39 | +{ |
| 40 | + std::string fleet_name = node.declare_parameter("fleet_name", std::string()); |
| 41 | + if (fleet_name.empty()) |
| 42 | + { |
| 43 | + RCLCPP_ERROR( |
| 44 | + node.get_logger(), |
| 45 | + "The fleet_name parameter must be specified!"); |
| 46 | + throw std::runtime_error("fleet_name parameter is missing"); |
| 47 | + } |
| 48 | + |
| 49 | + return fleet_name; |
| 50 | +} |
| 51 | + |
| 52 | +//============================================================================== |
| 53 | +rmf_traffic::agv::VehicleTraits get_traits_or_default(rclcpp::Node& node, |
| 54 | + const double default_v_nom, const double default_w_nom, |
| 55 | + const double default_a_nom, const double default_alpha_nom, |
| 56 | + const double default_r_f, const double default_r_v) |
| 57 | +{ |
| 58 | + const double v_nom = |
| 59 | + get_parameter_or_default(node, "linear_velocity", default_v_nom); |
| 60 | + const double w_nom = |
| 61 | + get_parameter_or_default(node, "angular_velocity", default_w_nom); |
| 62 | + const double a_nom = |
| 63 | + get_parameter_or_default(node, "linear_acceleration", default_a_nom); |
| 64 | + const double b_nom = |
| 65 | + get_parameter_or_default(node, "angular_acceleration", default_alpha_nom); |
| 66 | + const double r_f = |
| 67 | + get_parameter_or_default(node, "footprint_radius", default_r_f); |
| 68 | + const double r_v = |
| 69 | + get_parameter_or_default(node, "vicinity_radius", default_r_v); |
| 70 | + const bool reversible = |
| 71 | + get_parameter_or_default(node, "reversible", true); |
| 72 | + |
| 73 | + if (!reversible) |
| 74 | + std::cout << " ===== We have an irreversible robot" << std::endl; |
| 75 | + |
| 76 | + auto traits = rmf_traffic::agv::VehicleTraits{ |
| 77 | + {v_nom, a_nom}, |
| 78 | + {w_nom, b_nom}, |
| 79 | + rmf_traffic::Profile{ |
| 80 | + rmf_traffic::geometry::make_final_convex< |
| 81 | + rmf_traffic::geometry::Circle>(r_f), |
| 82 | + rmf_traffic::geometry::make_final_convex< |
| 83 | + rmf_traffic::geometry::Circle>(r_v) |
| 84 | + } |
| 85 | + }; |
| 86 | + |
| 87 | + traits.get_differential()->set_reversible(reversible); |
| 88 | + return traits; |
| 89 | +} |
| 90 | + |
| 91 | + |
| 92 | +//============================================================================== |
| 93 | +std::optional<rmf_battery::agv::BatterySystem> get_battery_system( |
| 94 | + rclcpp::Node& node, |
| 95 | + const double default_voltage, |
| 96 | + const double default_capacity, |
| 97 | + const double default_charging_current) |
| 98 | +{ |
| 99 | + const double voltage = |
| 100 | + get_parameter_or_default(node, "battery_voltage", default_voltage); |
| 101 | + const double capacity = |
| 102 | + get_parameter_or_default(node, "battery_capacity", default_capacity); |
| 103 | + const double charging_current = |
| 104 | + get_parameter_or_default( |
| 105 | + node, "battery_charging_current", default_charging_current); |
| 106 | + |
| 107 | + auto battery_system = rmf_battery::agv::BatterySystem::make( |
| 108 | + voltage, capacity, charging_current); |
| 109 | + |
| 110 | + return battery_system; |
| 111 | +} |
| 112 | + |
| 113 | +//============================================================================== |
| 114 | +std::optional<rmf_battery::agv::MechanicalSystem> get_mechanical_system( |
| 115 | + rclcpp::Node& node, |
| 116 | + const double default_mass, |
| 117 | + const double default_moment, |
| 118 | + const double default_friction) |
| 119 | +{ |
| 120 | + const double mass = |
| 121 | + get_parameter_or_default(node, "mass", default_mass); |
| 122 | + const double moment_of_inertia = |
| 123 | + get_parameter_or_default(node, "inertia", default_moment); |
| 124 | + const double friction = |
| 125 | + get_parameter_or_default(node, "friction_coefficient", default_friction); |
| 126 | + |
| 127 | + auto mechanical_system = rmf_battery::agv::MechanicalSystem::make( |
| 128 | + mass, moment_of_inertia, friction); |
| 129 | + |
| 130 | + return mechanical_system; |
| 131 | +} |
| 132 | + |
| 133 | +} // namespace rmf_fleet_adapter |
0 commit comments