Program Listing for File readonly_common.hpp
↰ Return to documentation for file (/tmp/ws/src/rmf_simulation/rmf_robot_sim_common/include/rmf_robot_sim_common/readonly_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_ROBOT_SIM_COMMON__READONLY_COMMON_HPP
#define RMF_ROBOT_SIM_COMMON__READONLY_COMMON_HPP
#include <rclcpp/rclcpp.hpp>
#include <rclcpp/logger.hpp>
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <mutex>
#include <string>
#include <Eigen/Geometry>
#include <rmf_fleet_msgs/msg/robot_state.hpp>
#include <rmf_building_map_msgs/msg/building_map.hpp>
#include <rmf_building_map_msgs/msg/level.hpp>
#include <rmf_building_map_msgs/msg/graph.hpp>
namespace rmf_readonly_common {
class ReadonlyCommon
{
public:
using BuildingMap = rmf_building_map_msgs::msg::BuildingMap;
using Level = rmf_building_map_msgs::msg::Level;
using Graph = rmf_building_map_msgs::msg::Graph;
using Location = rmf_fleet_msgs::msg::Location;
using Path = std::vector<Location>;
rclcpp::Node::SharedPtr ros_node;
void set_name(const std::string& name);
std::string get_name() const;
rclcpp::Logger logger();
template<typename SdfPtrT>
void read_sdf(SdfPtrT& sdf);
void init(rclcpp::Node::SharedPtr node);
void on_update(Eigen::Isometry3d& pose, double sim_time);
private:
std::string _name = "caddy"; // Placeholder
// Updated in each on_update() call
Eigen::Isometry3d _pose;
double _sim_time = 0.0;
rclcpp::Publisher<rmf_fleet_msgs::msg::RobotState>::SharedPtr _robot_state_pub;
rclcpp::Subscription<BuildingMap>::SharedPtr _building_map_sub;
rmf_fleet_msgs::msg::RobotState _robot_state_msg;
rmf_fleet_msgs::msg::RobotMode _current_mode;
bool _found_level = false;
bool _found_graph = false;
bool _initialized_graph = false;
bool _initialized_start = false;
// Store cache of BuildingMap
Path _path;
Level _level;
Graph _graph;
std::string _level_name = "L1";
std::size_t _nav_graph_index = 1;
std::string _start_wp_name = "caddy";
// The number of waypoints to add to the predicted path. Minimum is 1.
std::size_t _lookahead = 1;
std::size_t _start_wp;
std::vector<std::size_t> _next_wp;
std::unordered_map<std::size_t,
std::unordered_set<std::size_t>> _neighbor_map;
double _last_update_time = 0.0;
double _update_threshold = 0.5; // Update every 0.5s
double _waypoint_threshold = 2.0;
bool _merge_lane = false;
double _lane_threshold = 0.2; // Meters
std::string _current_task_id;
std::mutex _graph_update_mutex;
void map_cb(const BuildingMap::SharedPtr msg);
void initialize_graph();
double compute_ds(
const Eigen::Isometry3d& pose,
const std::size_t& wp);
void initialize_start(const Eigen::Isometry3d& pose);
std::size_t get_next_waypoint(const std::size_t start_wp,
const Eigen::Vector3d& heading);
ReadonlyCommon::Path compute_path(
const Eigen::Isometry3d& pose);
};
template<typename SdfPtrT>
void ReadonlyCommon::read_sdf(SdfPtrT& sdf)
{
// Getting sdf elements
if (sdf->HasElement("level_name"))
_level_name = sdf->template Get<std::string>("level_name");
RCLCPP_INFO(logger(), "Setting level name to: %s", _level_name.c_str());
if (sdf->HasElement("graph_index"))
_nav_graph_index = sdf->template Get<std::size_t>("graph_index");
RCLCPP_INFO(logger(), "Setting nav graph index: %d", _nav_graph_index);
if (sdf->HasElement("spawn_waypoint"))
_start_wp_name = sdf->template Get<std::string>("spawn_waypoint");
RCLCPP_INFO(logger(), "Setting start wp name: %s", _start_wp_name.c_str());
if (sdf->HasElement("look_ahead"))
_lookahead = sdf->template Get<std::size_t>("look_ahead");
_lookahead = _lookahead < 1 ? 1 : _lookahead;
_next_wp.resize(_lookahead);
_path.resize(_lookahead);
RCLCPP_INFO(logger(), "Setting lookahead: %d", _lookahead);
if (sdf->HasElement("update_rate"))
_update_threshold = 1.0 / sdf->template Get<double>("update_rate");
RCLCPP_INFO(logger(), "Setting update threshold: %f", _update_threshold);
if (sdf->HasElement("waypoint_threshold"))
_waypoint_threshold = sdf->template Get<double>("waypoint_threshold");
RCLCPP_INFO(logger(), "Setting waypoint threshold: %f", _waypoint_threshold);
if (sdf->HasElement("merge_lane"))
_merge_lane = sdf->template Get<bool>("merge_lane");
RCLCPP_INFO(
logger(),
"Setting merge lane: %s",
std::to_string(_merge_lane).c_str());
if (sdf->HasElement("lane_threshold"))
_lane_threshold = sdf->template Get<double>("lane_threshold");
RCLCPP_INFO(logger(), "Setting lane threshold: %f", _lane_threshold);
}
} // namespace rmf_readonly_common
#endif