Skip to content

Commit

Permalink
Slotcar plugin package move and utils cleanup (#5)
Browse files Browse the repository at this point in the history 
* Move slotcar to rmf_robot_sim_*

Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net>

* Fix namespace usage

Signed-off-by: Geoffrey Biggs <gbiggs@killbots.net>

* Refactor and cleanup utils (#6)

Signed-off-by: Luca Della Vedova <luca@openrobotics.org>

Co-authored-by: Luca Della Vedova <luca@openrobotics.org>
  • Loading branch information
@gbiggs @luca-della-vedova
gbiggs and luca-della-vedova committed Apr 5, 2021
1 parent 6fe9789 commit 5bae14d
Show file tree
Hide file tree
Showing 19 changed files with 168 additions and 246 deletions.
40 changes: 17 additions & 23 deletions rmf_building_sim_common/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,9 +22,6 @@ endif()
# find dependencies
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(Eigen3 REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(rmf_fleet_msgs REQUIRED)
Expand All @@ -35,24 +32,16 @@ find_package(menge QUIET)

include(GNUInstallDirs)

add_library(slotcar_common SHARED ${PROJECT_SOURCE_DIR}/src/slotcar_common.cpp)
###############################
# Utils #
###############################

ament_target_dependencies(slotcar_common
Eigen3
rmf_fleet_msgs
rmf_building_map_msgs
rclcpp
std_msgs
geometry_msgs
tf2_ros
)
add_library(rmf_building_sim_utils SHARED ${PROJECT_SOURCE_DIR}/src/utils.cpp)

target_include_directories(slotcar_common
target_include_directories(rmf_building_sim_utils
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
${EIGEN3_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)

###############################
Expand All @@ -73,6 +62,10 @@ target_include_directories(door_common
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
)

target_link_libraries(door_common
rmf_building_sim_utils
)

###############################
# lift stuff
###############################
Expand All @@ -86,13 +79,16 @@ ament_target_dependencies(lift_common
rmf_lift_msgs
)


target_include_directories(lift_common
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
)

target_link_libraries(lift_common
rmf_building_sim_utils
)

###############################
# crowd simulator stuff
###############################
Expand Down Expand Up @@ -129,17 +125,15 @@ endif()
###############################
# install stuff
###############################
ament_export_include_directories(include)

ament_export_targets(slotcar_common HAS_LIBRARY_TARGET)
ament_export_include_directories(include)
ament_export_targets(rmf_building_sim_utils HAS_LIBRARY_TARGET)
ament_export_targets(door_common HAS_LIBRARY_TARGET)
ament_export_targets(lift_common HAS_LIBRARY_TARGET)

ament_export_dependencies(Eigen3)

install(
TARGETS slotcar_common
EXPORT slotcar_common
TARGETS rmf_building_sim_utils
EXPORT rmf_building_sim_utils
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib
)
Expand Down
119 changes: 10 additions & 109 deletions rmf_building_sim_common/include/rmf_building_sim_common/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,16 +6,6 @@

namespace rmf_building_sim_common {

// TODO(MXG): Refactor the use of this function to replace it with
// compute_desired_rate_of_change()
double compute_ds(
double s_target,
double v_actual,
double v_max,
double accel_nom,
double accel_max,
double dt);

struct MotionParams
{
double v_max = 0.2;
Expand All @@ -26,98 +16,22 @@ struct MotionParams
};

//==============================================================================
// TODO(MXG): Refactor the use of this function to replace it with
// compute_desired_rate_of_change()
double compute_ds(
double s_target,
double v_actual,
const double v_max,
const double accel_nom,
const double accel_max,
const double dt)
{
double sign = 1.0;
if (s_target < 0.0)
{
// Limits get confusing when we need to go backwards, so we'll flip signs
// here so that we pretend the target is forwards
s_target *= -1.0;
v_actual *= -1.0;
sign = -1.0;
}

// We should try not to shoot past the targstd::vector<event::ConnectionPtr> connections;et
double next_s = s_target / dt;

// Test velocity limit
next_s = std::min(next_s, v_max);

// Test acceleration limit
next_s = std::min(next_s, accel_nom * dt + v_actual);

if (v_actual > 0.0 && s_target > 0.0)
{
// This is what our deceleration should be if we want to begin a constant
// deceleration from now until we reach the goal
double deceleration = pow(v_actual, 2) / s_target;
deceleration = std::min(deceleration, accel_max);

if (accel_nom <= deceleration)
{
// If the smallest constant deceleration for reaching the goal is
// greater than
next_s = -deceleration * dt + v_actual;
}
}

// Flip the sign the to correct direction before returning the value
return sign * next_s;
}
const double dt);

//==============================================================================
double compute_desired_rate_of_change(
double _s_target,
double _v_actual,
const MotionParams& _motion_params,
const double _dt)
{
double sign = 1.0;
if (_s_target < 0.0)
{
// Limits get confusing when we need to go backwards, so we'll flip signs
// here so that we pretend the target is forwards
_s_target *= -1.0;
_v_actual *= -1.0;
sign = -1.0;
}

// We should try not to shoot past the target
double v_next = _s_target / _dt;

// Test velocity limit
v_next = std::min(v_next, _motion_params.v_max);

// Test acceleration limit
v_next = std::min(v_next, _motion_params.a_nom * _dt + _v_actual);

if (_v_actual > 0.0 && _s_target > 0.0)
{
// This is what our deceleration should be if we want to begin a constant
// deceleration from now until we reach the goal
double deceleration = pow(_v_actual, 2) / (2.0 * _s_target);
deceleration = std::min(deceleration, _motion_params.a_max);

if (_motion_params.a_nom <= deceleration)
{
// If the smallest constant deceleration for reaching the goal is
// greater than the nominal acceleration, then we should begin
// decelerating right away so that we can smoothly reach the goal while
// decelerating as close to the nominal acceleration as possible.
v_next = -deceleration * _dt + _v_actual;
}
}

// Flip the sign the to correct direction before returning the value
return sign * v_next;
}
double _s_target,
double _v_actual,
const MotionParams& _motion_params,
const double _dt);

//==============================================================================
template<typename SdfPtrT, typename SdfElementPtrT>
Expand All @@ -136,20 +50,7 @@ bool get_element_required(
return true;
}

/*
double compute_desired_rate_of_change(
double _s_target,
double _v_actual,
const MotionParams& _motion_params,
const double _dt);
bool get_element_required(
const sdf::ElementPtr& _sdf,
const std::string& _element_name,
sdf::ElementPtr& _element);
*/

////////////////////////////////////////////////////////////////////////////////////////////////////
//==============================================================================
template<typename T, typename SdfPtrT>
bool get_sdf_attribute_required(SdfPtrT& sdf, const std::string& attribute_name,
T& value)
Expand Down Expand Up @@ -177,7 +78,7 @@ bool get_sdf_attribute_required(SdfPtrT& sdf, const std::string& attribute_name,
return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//==============================================================================
template<typename T, typename SdfPtrT>
bool get_sdf_param_required(SdfPtrT& sdf, const std::string& parameter_name,
T& value)
Expand All @@ -204,7 +105,7 @@ bool get_sdf_param_required(SdfPtrT& sdf, const std::string& parameter_name,
return false;
}

////////////////////////////////////////////////////////////////////////////////////////////////////
//==============================================================================
template<typename T, typename SdfPtrT>
void get_sdf_param_if_available(SdfPtrT& sdf, const std::string& parameter_name,
T& value)
Expand Down
2 changes: 0 additions & 2 deletions rmf_building_sim_common/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -23,8 +23,6 @@
<depend>rmf_door_msgs</depend>
<depend>rmf_lift_msgs</depend>
<depend>rmf_building_map_msgs</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>menge</depend>
Expand Down
20 changes: 2 additions & 18 deletions rmf_building_sim_common/src/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -3,7 +3,7 @@

#include <rmf_building_sim_common/utils.hpp>

namespace rmf_building_sim_gazebo_plugins {
namespace rmf_building_sim_common {

//==============================================================================
double compute_ds(
Expand Down Expand Up @@ -98,20 +98,4 @@ double compute_desired_rate_of_change(
// Flip the sign to the correct direction before returning the value
return sign * v_next;
}

//==============================================================================
bool get_element_required(
const sdf::ElementPtr& _sdf,
const std::string& _element_name,
sdf::ElementPtr& _element)
{
if (!_sdf->HasElement(_element_name))
{
std::cerr << "Element [" << _element_name << "] not found" << std::endl;
return false;
}
_element = _sdf->GetElement(_element_name);
return true;
}

} // namespace rmf_building_sim_gazebo_plugins
} // namespace rmf_building_sim_common
28 changes: 2 additions & 26 deletions rmf_building_sim_gazebo_plugins/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,6 @@ find_package(gazebo_ros REQUIRED)
find_package(gazebo_dev REQUIRED)
find_package(OpenCV REQUIRED )
find_package(gazebo_msgs REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(rmf_fleet_msgs REQUIRED)
Expand All @@ -39,32 +37,10 @@ find_package(rmf_building_sim_common REQUIRED)
find_package(menge QUIET)

# TODO this is a dependency of rmf_building_sim_common, it shouldn't be needed
find_package(Eigen3 REQUIRED)
#find_package(Eigen3 REQUIRED)

include(GNUInstallDirs)

add_library(slotcar SHARED ${PROJECT_SOURCE_DIR}/src/slotcar.cpp)

ament_target_dependencies(slotcar
Eigen3
rmf_building_sim_common
rmf_fleet_msgs
rclcpp
gazebo_ros
std_msgs
geometry_msgs
tf2_ros
rmf_building_map_msgs
)

target_include_directories(slotcar
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
${EIGEN3_INCLUDE_DIRS}
${GAZEBO_INCLUDE_DIRS}
)

###############################
# door stuff
###############################
Expand Down Expand Up @@ -203,7 +179,7 @@ endif()
###############################

install(
TARGETS slotcar door lift toggle_floors toggle_charging thumbnail_generator
TARGETS door lift toggle_floors toggle_charging thumbnail_generator
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
)
Expand Down
2 changes: 0 additions & 2 deletions rmf_building_sim_gazebo_plugins/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,6 @@
<depend>rmf_fleet_msgs</depend>
<depend>rmf_door_msgs</depend>
<depend>rmf_lift_msgs</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>
<depend>rmf_building_sim_common</depend>
Expand Down

0 comments on commit 5bae14d

Please sign in to comment.