Program Listing for File slotcar_common.hpp
↰ Return to documentation for file (/tmp/ws/src/rmf_simulation/rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp
)
/*
* Copyright (C) 2020 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.
*
*/
#ifndef RMF_BUILDING_SIM_COMMON__SLOTCAR_COMMON_HPP
#define RMF_BUILDING_SIM_COMMON__SLOTCAR_COMMON_HPP
#include <sstream>
#include <rclcpp/rclcpp.hpp>
#include <Eigen/Geometry>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <rmf_fleet_msgs/msg/robot_mode.hpp>
#include <rmf_fleet_msgs/msg/robot_state.hpp>
#include <rmf_fleet_msgs/msg/path_request.hpp>
#include <rmf_fleet_msgs/msg/pause_request.hpp>
#include <rmf_fleet_msgs/msg/mode_request.hpp>
#include <rmf_building_map_msgs/msg/building_map.hpp>
namespace rmf_robot_sim_common {
struct SlotcarTrajectory
{
SlotcarTrajectory(const Eigen::Isometry3d& _pose)
: pose(_pose)
{}
SlotcarTrajectory() {}
// positions
Eigen::Isometry3d pose;
// Maximum speed for the lane approaching this waypoint
std::optional<double> approach_speed_limit;
};
// steering type constants
enum class SteeringType
{
DIFF_DRIVE,
ACKERMANN
};
class SlotcarCommon
{
public:
struct UpdateResult
{
double v = 0.0; // Target displacement in X (forward)
double w = 0.0; // Target displacement in yaw
double target_linear_speed_now = 0.0; // Target linear speed now
double target_linear_speed_destination = 0.0; // Target linear speed at destination
std::optional<double> max_speed = std::nullopt; // Maximum speed allowed while navigating
};
SlotcarCommon();
rclcpp::Logger logger() const;
template<typename SdfPtrT>
void read_sdf(SdfPtrT& sdf);
void set_model_name(const std::string& model_name);
std::string model_name() const;
void init_ros_node(const rclcpp::Node::SharedPtr node);
UpdateResult update(const Eigen::Isometry3d& pose,
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time);
bool emergency_stop(const std::vector<Eigen::Vector3d>& obstacle_positions,
const Eigen::Vector3d& current_heading);
std::array<double, 2> calculate_control_signals(const std::array<double,
2>& curr_velocities,
const std::pair<double, double>& displacements,
const double dt,
const double target_velocity_now = 0.0,
const double target_velocity_at_dest = 0.0,
const std::optional<double>& linear_speed_limit = std::nullopt) const;
std::array<double, 2> calculate_joint_control_signals(
const std::array<double, 2>& w_tire,
const std::pair<double, double>& displacements,
const double dt,
const double target_linear_speed_now = 0.0,
const double target_linear_speed_destination = 0.0,
const std::optional<double>& linear_speed_limit = std::nullopt) const;
void charge_state_cb(const std::string& name, bool selected);
void publish_robot_state(const double time);
Eigen::Vector3d get_lookahead_point() const;
bool display_markers = false; // Ignition only: toggles display of waypoint and lookahead markers
using PathRequestCallback =
std::function<void(const rmf_fleet_msgs::msg::PathRequest::SharedPtr)>;
void set_path_request_callback(PathRequestCallback cb)
{ _path_request_callback = cb; }
private:
// Parameters needed for power dissipation and charging calculations
// Default values may be overriden using values from sdf file
struct PowerParams
{
double nominal_voltage = 12; // V
double nominal_capacity = 24; // Ah
double charging_current = 2; // A
double mass = 20;
double inertia = 10;
double friction_coefficient = 0.3;
double nominal_power = 10;
};
struct ChargerWaypoint
{
double x;
double y;
ChargerWaypoint(double x, double y)
: x(x), y(y)
{
}
};
// Constants for update rate of tf2 and robot_state topic
static constexpr float TF2_RATE = 100.0;
static constexpr float STATE_TOPIC_RATE = 2.0;
// Initial distance threshold over which a fleet adapter error is reported
static constexpr float INITIAL_DISTANCE_THRESHOLD = 1.0;
rclcpp::Node::SharedPtr _ros_node;
double _last_update_time = 0.0;
double last_tf2_pub = 0.0;
double last_topic_pub = 0.0;
std::size_t _sequence = 0;
std::vector<SlotcarTrajectory> trajectory;
std::size_t _traj_wp_idx = 0;
rmf_fleet_msgs::msg::PauseRequest pause_request;
std::mutex _mutex;
std::string _model_name;
bool _emergency_stop = false;
bool _adapter_error = false;
bool _initialized_pose = false; // True if at least 1 call to update() has been made
Eigen::Isometry3d _old_pose; // Pose at previous time step
// Assumes robot is stationary upon initialization
Eigen::Vector3d _old_lin_vel = Eigen::Vector3d::Zero(); // Linear velocity at previous time step
double _old_ang_vel = 0.0; // Angular velocity at previous time step
bool _was_rotating; // Whether robot was rotating towards its next target in previous time step
Eigen::Isometry3d _pose; // Pose at current time step
int _rot_dir = 1; // Current direction of rotation
std::unordered_map<std::string, double> _level_to_elevation;
bool _initialized_levels = false;
std::string _last_known_level = "";
std::shared_ptr<tf2_ros::TransformBroadcaster> _tf2_broadcaster;
rclcpp::Publisher<rmf_fleet_msgs::msg::RobotState>::SharedPtr _robot_state_pub;
rclcpp::Subscription<rmf_fleet_msgs::msg::PathRequest>::SharedPtr _traj_sub;
rclcpp::Subscription<rmf_fleet_msgs::msg::PauseRequest>::SharedPtr _pause_sub;
rclcpp::Subscription<rmf_fleet_msgs::msg::ModeRequest>::SharedPtr _mode_sub;
rclcpp::Subscription<rmf_building_map_msgs::msg::BuildingMap>::SharedPtr
_building_map_sub;
rmf_fleet_msgs::msg::RobotMode _current_mode;
SteeringType _steering_type = SteeringType::DIFF_DRIVE;
std::string _current_task_id;
std::vector<rmf_fleet_msgs::msg::Location> _remaining_path;
// Vehicle dynamic constants
// TODO(MXG): Consider fetching these values from model data
// Radius of a tire
double _tire_radius = 0.1;
// Distance of a tire from the origin
double _base_width = 0.52;
double _nominal_drive_speed = 0.5; // nominal robot velocity (m/s)
double _nominal_drive_acceleration = 0.05; // nominal robot forward acceleration (m/s^2)
double _max_drive_acceleration = 0.1; // maximum robot forward acceleration (m/s^2)
double _nominal_turn_speed = M_PI / 8.0; // nominal robot turning speed (half a rotation per 8 seconds)
double _nominal_turn_acceleration = M_PI / 10.0; // nominal robot turning acceleration (rad/s^2)
double _max_turn_acceleration = M_PI; // maximum robot turning acceleration (rad/s^2)
double _stop_distance = 1.0;
double _stop_radius = 1.0;
double _min_turning_radius = -1.0; // minimum turning radius, will use a formula if negative
double _turning_right_angle_mul_offset = 1.0; // if _min_turning_radius is computed, this value multiplies it
bool _reversible = true; // true if the robot can drive backwards
PowerParams _params;
bool _enable_charge = true;
bool _enable_instant_charge = false;
bool _enable_drain = true;
// Used for comparing with name argument of charge_state_cb to identify button selected
const std::string _enable_charge_str = "_enable_charge";
const std::string _enable_instant_charge_str = "_enable_instant_charge";
const std::string _enable_drain_str = "_enable_drain";
const double _soc_max = 1.0;
double _soc = _soc_max;
std::unordered_map<std::string, std::vector<ChargerWaypoint>>
_charger_waypoints;
// Straight line distance to charging waypoint within which charging can occur
static constexpr double _charger_dist_thres = 0.3;
bool _docking = false;
Eigen::Vector3d _lookahead_point;
double _lookahead_distance = 8.0;
PathRequestCallback _path_request_callback = nullptr;
std::string get_level_name(const double z);
double compute_change_in_rotation(
const Eigen::Vector3d& heading_vec,
const Eigen::Vector3d& dpos,
const Eigen::Vector3d* traj_vec = nullptr,
double* const dir = nullptr) const;
void publish_tf2(const rclcpp::Time& t);
void publish_state_topic(const rclcpp::Time& t);
bool path_request_valid(
const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);
void path_request_cb(const rmf_fleet_msgs::msg::PathRequest::SharedPtr msg);
void pause_request_cb(const rmf_fleet_msgs::msg::PauseRequest::SharedPtr msg);
void mode_request_cb(const rmf_fleet_msgs::msg::ModeRequest::SharedPtr msg);
void map_cb(const rmf_building_map_msgs::msg::BuildingMap::SharedPtr msg);
UpdateResult update_diff_drive(
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time);
UpdateResult update_ackermann(
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time);
bool near_charger(const Eigen::Isometry3d& pose) const;
double compute_charge(const double run_time) const;
double compute_discharge(
const Eigen::Vector3d lin_vel, const double ang_vel,
const Eigen::Vector3d lin_acc, const double ang_acc,
const double run_time) const;
};
template<typename SdfPtrT, typename valueT>
bool get_element_val_if_present(
SdfPtrT& _sdf,
const std::string& _element_name,
valueT& _val)
{
if (!_sdf->HasElement(_element_name))
{
return false;
}
_val = _sdf->template Get<valueT>(_element_name);
return true;
}
template<typename SdfPtrT>
void SlotcarCommon::read_sdf(SdfPtrT& sdf)
{
std::string steering_type;
get_element_val_if_present<SdfPtrT, std::string>(sdf, "steering",
steering_type);
if (steering_type == "ackermann")
{
_steering_type = SteeringType::ACKERMANN;
_reversible = false;
}
else if (steering_type == "diff_drive")
{
_steering_type = SteeringType::DIFF_DRIVE;
}
RCLCPP_INFO(
logger(),
"Vehicle uses %s steering", steering_type.c_str());
get_element_val_if_present<SdfPtrT, double>(sdf, "nominal_drive_speed",
this->_nominal_drive_speed);
RCLCPP_INFO(
logger(),
"Setting nominal drive speed to: %f",
_nominal_drive_speed);
get_element_val_if_present<SdfPtrT, double>(sdf,
"nominal_drive_acceleration", this->_nominal_drive_acceleration);
RCLCPP_INFO(
logger(),
"Setting nominal drive acceleration to: %f",
_nominal_drive_acceleration);
get_element_val_if_present<SdfPtrT, double>(sdf,
"max_drive_acceleration", this->_max_drive_acceleration);
RCLCPP_INFO(
logger(),
"Setting max drive acceleration to: %f",
_max_drive_acceleration);
get_element_val_if_present<SdfPtrT, double>(sdf,
"nominal_turn_speed", this->_nominal_turn_speed);
RCLCPP_INFO(
logger(),
"Setting nominal turn speed to: %f",
_nominal_turn_speed);
get_element_val_if_present<SdfPtrT, double>(sdf,
"nominal_turn_acceleration", this->_nominal_turn_acceleration);
RCLCPP_INFO(
logger(),
"Setting nominal turn acceleration to: %f",
_nominal_turn_acceleration);
get_element_val_if_present<SdfPtrT, double>(sdf,
"max_turn_acceleration", this->_max_turn_acceleration);
RCLCPP_INFO(
logger(),
"Setting max turn acceleration to: %f",
_max_turn_acceleration);
get_element_val_if_present<SdfPtrT, double>(sdf,
"min_turning_radius", this->_min_turning_radius);
RCLCPP_INFO(
logger(),
"Setting minimum turning radius to: %f",
_min_turning_radius);
get_element_val_if_present<SdfPtrT, double>(sdf,
"turning_right_angle_mul_offset", this->_turning_right_angle_mul_offset);
RCLCPP_INFO(
logger(),
"Setting turning right angle multiplier offset to: %f",
_turning_right_angle_mul_offset);
get_element_val_if_present<SdfPtrT, double>(sdf,
"stop_distance", this->_stop_distance);
RCLCPP_INFO(logger(), "Setting stop distance to: %f", _stop_distance);
get_element_val_if_present<SdfPtrT, double>(sdf,
"stop_radius", this->_stop_radius);
RCLCPP_INFO(logger(), "Setting stop radius to: %f", _stop_radius);
get_element_val_if_present<SdfPtrT, double>(sdf,
"tire_radius", this->_tire_radius);
RCLCPP_INFO(logger(), "Setting tire radius to: %f", _tire_radius);
get_element_val_if_present<SdfPtrT, double>(sdf,
"base_width", this->_base_width);
RCLCPP_INFO(logger(), "Setting base width to: %f", _base_width);
get_element_val_if_present<SdfPtrT, bool>(sdf,
"reversible", this->_reversible);
RCLCPP_INFO(logger(), "Setting reversible to: %d", _reversible);
get_element_val_if_present<SdfPtrT, double>(sdf,
"nominal_voltage", this->_params.nominal_voltage);
RCLCPP_INFO(
logger(),
"Setting nominal voltage to: %f",
_params.nominal_voltage);
get_element_val_if_present<SdfPtrT, double>(sdf,
"nominal_capacity", this->_params.nominal_capacity);
RCLCPP_INFO(
logger(),
"Setting nominal capacity to: %f",
_params.nominal_capacity);
get_element_val_if_present<SdfPtrT, double>(sdf,
"charging_current", this->_params.charging_current);
RCLCPP_INFO(
logger(),
"Setting charging current to: %f",
_params.charging_current);
get_element_val_if_present<SdfPtrT, double>(sdf,
"mass", this->_params.mass);
RCLCPP_INFO(logger(), "Setting mass to: %f", _params.mass);
get_element_val_if_present<SdfPtrT, double>(sdf,
"inertia", this->_params.inertia);
RCLCPP_INFO(
logger(),
"Setting inertia to: %f",
_params.inertia);
get_element_val_if_present<SdfPtrT, double>(sdf,
"friction_coefficient", this->_params.friction_coefficient);
RCLCPP_INFO(
logger(),
"Setting friction coefficient to: %f",
_params.friction_coefficient);
get_element_val_if_present<SdfPtrT, double>(sdf,
"nominal_power", this->_params.nominal_power);
RCLCPP_INFO(
logger(),
"Setting nominal power to: %f",
_params.nominal_power);
get_element_val_if_present<SdfPtrT, double>(sdf,
"lookahead_distance", this->_lookahead_distance);
RCLCPP_INFO(
logger(),
"Setting lookahead distance to: %f",
_lookahead_distance);
get_element_val_if_present<SdfPtrT, bool>(sdf,
"display_markers", this->display_markers);
RCLCPP_INFO(logger(), "Setting display_markers to: %d", display_markers);
// Charger Waypoint coordinates are in child element of top level world element
if (sdf->GetParent() && sdf->GetParent()->GetParent())
{
auto parent = sdf->GetParent()->GetParent();
if (parent->HasElement("rmf_charger_waypoints"))
{
auto waypoints = parent->GetElement("rmf_charger_waypoints");
if (waypoints->HasElement("rmf_vertex"))
{
auto waypoint = waypoints->GetElement("rmf_vertex");
while (waypoint)
{
if (waypoint->HasAttribute("x") && waypoint->HasAttribute("y") &&
waypoint->HasAttribute("level"))
{
std::string lvl_name;
double x, y;
waypoint->GetAttribute("x")->Get(x);
waypoint->GetAttribute("y")->Get(y);
waypoint->GetAttribute("level")->Get(lvl_name);
_charger_waypoints[lvl_name].push_back(ChargerWaypoint(x, y));
}
waypoint = waypoint->GetNextElement("rmf_vertex");
}
}
}
else
{
RCLCPP_INFO(logger(), "No charger waypoints found.");
}
}
RCLCPP_INFO(logger(), "Setting name to: %s", _model_name.c_str());
}
} // namespace rmf_robot_sim_common
#endif // RMF_BUILDING_SIM_COMMON__SLOTCAR_COMMON_HPP