Skip to content

Commit

Permalink
Remove unused code from slotcar / readonly (#140)
Browse files Browse the repository at this point in the history 
Signed-off-by: Luca Della Vedova <[email protected]>
  • Loading branch information
@luca-della-vedova
luca-della-vedova authored Dec 23, 2024
1 parent 6650208 commit 40a2262
Show file tree
Hide file tree
Showing 5 changed files with 3 additions and 164 deletions.
3 changes: 0 additions & 3 deletions rmf_robot_sim_common/include/rmf_robot_sim_common/readonly_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,7 +24,6 @@
#include <memory>
#include <unordered_map>
#include <unordered_set>
#include <mutex>
#include <string>

#include <Eigen/Geometry>
Expand Down Expand Up @@ -99,8 +98,6 @@ class ReadonlyCommon

std::string _current_task_id;

std::mutex _graph_update_mutex;

void map_cb(const BuildingMap::SharedPtr msg);
void initialize_graph();
double compute_ds(
Expand Down
10 changes: 0 additions & 10 deletions rmf_robot_sim_common/include/rmf_robot_sim_common/slotcar_common.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,14 +97,6 @@ class SlotcarCommon
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);
Expand Down Expand Up @@ -161,8 +153,6 @@ class SlotcarCommon

rmf_fleet_msgs::msg::PauseRequest pause_request;

std::mutex _mutex;

std::string _model_name;
bool _emergency_stop = false;
bool _adapter_error = false;
Expand Down
98 changes: 0 additions & 98 deletions rmf_robot_sim_common/include/rmf_robot_sim_common/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -76,7 +76,6 @@ struct MotionParams
double a_max = 0.1;
double a_nom = 0.08;
double dx_min = 0.01;
double f_max = 10000000.0;
};

double compute_desired_rate_of_change(
Expand All @@ -91,103 +90,6 @@ rclcpp::Time simulation_now(double t);

void sanitize_node_name(std::string& node_name);

////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename SdfPtrT, typename SdfElementPtrT>
bool get_element_required(
SdfPtrT& _sdf,
const std::string& _element_name,
SdfElementPtrT& _element)
{
if (!_sdf->HasElement(_element_name))
{
std::cerr << "Element [" << _element_name << "] not found" << std::endl;
return false;
}
// using GetElementImpl() because for sdf::v9 GetElement() is not const
_element = _sdf->GetElementImpl(_element_name);
return true;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename T, typename SdfPtrT>
bool get_sdf_attribute_required(SdfPtrT& sdf, const std::string& attribute_name,
T& value)
{
if (sdf->HasAttribute(attribute_name))
{
if (sdf->GetAttribute(attribute_name)->Get(value))
{
std::cout << "Using specified attribute value [" << value
<< "] for property [" << attribute_name << "]"
<< std::endl;
return true;
}
else
{
std::cerr << "Failed to parse sdf attribute for [" << attribute_name
<< "]" << std::endl;
}
}
else
{
std::cerr << "Attribute [" << attribute_name << "] not found" << std::endl;
}

return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename T, typename SdfPtrT>
bool get_sdf_param_required(SdfPtrT& sdf, const std::string& parameter_name,
T& value)
{
if (sdf->HasElement(parameter_name))
{
if (sdf->GetElement(parameter_name)->GetValue()->Get(value))
{
std::cout << "Using specified value [" << value << "] for property ["
<< parameter_name << "]" << std::endl;
return true;
}
else
{
std::cerr << "Failed to parse sdf value for [" << parameter_name << "]"
<<std::endl;
}
}
else
{
std::cerr << "Property [" << parameter_name << "] not found" << std::endl;
}

return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename T, typename SdfPtrT>
void get_sdf_param_if_available(SdfPtrT& sdf, const std::string& parameter_name,
T& value)
{
if (sdf->HasElement(parameter_name))
{
if (sdf->GetElement(parameter_name)->GetValue()->Get(value))
{
std::cout << "Using specified value [" << value << "] for property ["
<< parameter_name << "]" << std::endl;
}
else
{
std::cerr << "Failed to parse sdf value for [" << parameter_name
<< "]" << std::endl;
}
}
else
{
std::cout << "Using default value [" << value << "] for property ["
<< parameter_name << "]" << std::endl;
}
}

/////////////////////////////////////////////////////////////////////////////////////////////////////
template<typename ResultMsgT>
std::shared_ptr<ResultMsgT> make_response(uint8_t status,
Expand Down
3 changes: 0 additions & 3 deletions rmf_robot_sim_common/src/readonly_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -164,8 +164,6 @@ void ReadonlyCommon::initialize_graph()
if (!_found_graph)
return;

std::lock_guard<std::mutex> lock(_graph_update_mutex);

_initialized_graph = false;

_graph = _level.nav_graphs[_nav_graph_index];
Expand Down Expand Up @@ -328,7 +326,6 @@ ReadonlyCommon::Path ReadonlyCommon::compute_path(

for (std::size_t i = 0; i < _lookahead; i++)
{
std::lock_guard<std::mutex> lock(_graph_update_mutex);
auto wp = get_next_waypoint(start_wp, heading);
_next_wp[i] = wp;
// Add to path here
Expand Down
53 changes: 3 additions & 50 deletions rmf_robot_sim_common/src/slotcar_common.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -264,7 +264,6 @@ void SlotcarCommon::path_request_cb(
{
if (path_request_valid(msg) == false)
return;
std::lock_guard<std::mutex> lock(_mutex);

const auto old_path = _remaining_path;

Expand Down Expand Up @@ -335,7 +334,6 @@ void SlotcarCommon::pause_request_cb(
if (msg->robot_name != _model_name)
return;

std::lock_guard<std::mutex> lock(_mutex);
pause_request = *msg;
}

Expand All @@ -356,8 +354,7 @@ std::array<double, 2> SlotcarCommon::calculate_control_signals(
max_lin_vel,
_max_drive_acceleration,
_nominal_drive_acceleration,
0.01,
10000000.0};
0.01};
const double v_target = rmf_plugins_utils::compute_desired_rate_of_change(
displacements.first,
v_robot,
Expand All @@ -370,8 +367,7 @@ std::array<double, 2> SlotcarCommon::calculate_control_signals(
_nominal_turn_speed,
_max_turn_acceleration,
_nominal_turn_acceleration,
0.01,
10000000.0};
0.01};
const double w_target = rmf_plugins_utils::compute_desired_rate_of_change(
displacements.second,
w_robot,
Expand All @@ -383,50 +379,10 @@ std::array<double, 2> SlotcarCommon::calculate_control_signals(
return std::array<double, 2>{v_target, w_target};
}

std::array<double, 2> SlotcarCommon::calculate_joint_control_signals(
const std::array<double, 2>& w_tire,
const std::pair<double, double>& displacements,
const double dt,
const double linear_speed_target_now,
const double linear_speed_target_destination,
const std::optional<double>& linear_speed_limit) const
{
std::array<double, 2> curr_velocities;
curr_velocities[0] = (w_tire[0] + w_tire[1]) * _tire_radius / 2.0;
curr_velocities[1] = (w_tire[1] - w_tire[0]) * _tire_radius / _base_width;

std::array<double, 2> new_velocities = calculate_control_signals(
curr_velocities, displacements, dt, linear_speed_target_now,
linear_speed_target_destination,
linear_speed_limit);

std::array<double, 2> joint_signals;
for (std::size_t i = 0; i < 2; ++i)
{
const double yaw_sign = i == 0 ? -1.0 : 1.0;
joint_signals[i] = (new_velocities[0] / _tire_radius) + (yaw_sign *
new_velocities[1] * _base_width / (2.0 * _tire_radius));
}
return joint_signals;
}

std::string to_str(uint32_t type)
{
if (rmf_fleet_msgs::msg::PauseRequest::TYPE_RESUME == type)
return "resume";
else if (rmf_fleet_msgs::msg::PauseRequest::TYPE_PAUSE_IMMEDIATELY == type)
return "pause immediately";
else if (rmf_fleet_msgs::msg::PauseRequest::TYPE_PAUSE_AT_CHECKPOINT == type)
return "pause at checkpoint";

return "UNKNOWN: " + std::to_string(type) + "??";
}

SlotcarCommon::UpdateResult SlotcarCommon::update(const Eigen::Isometry3d& pose,
const std::vector<Eigen::Vector3d>& obstacle_positions,
const double time)
{
std::lock_guard<std::mutex> lock(_mutex);
_pose = pose;
publish_robot_state(time);

Expand Down Expand Up @@ -962,10 +918,7 @@ double SlotcarCommon::compute_change_in_rotation(

void SlotcarCommon::publish_robot_state(const double time)
{
const int32_t t_sec = static_cast<int32_t>(time);
const uint32_t t_nsec =
static_cast<uint32_t>((time-static_cast<double>(t_sec)) *1e9);
const rclcpp::Time ros_time{t_sec, t_nsec, RCL_ROS_TIME};
const rclcpp::Time ros_time = rmf_plugins_utils::simulation_now(time);
if ((time - last_tf2_pub) > (1.0 / TF2_RATE))
{
// Publish tf2
Expand Down

0 comments on commit 40a2262

Please sign in to comment.