Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Trajectory processing, port to ROS 2 #63

Merged
merged 9 commits into from
Jun 12, 2019
Merged

Trajectory processing, port to ROS 2 #63

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
648862a
Port trajectory_processing to ROS 2
vmayoral Mar 31, 2019
c512827
trajectory_processing, fix logging according to https://github.com/ro…
vmayoral Mar 31, 2019
4fc9f93
Merge branch 'moveit2' into trajectory-processing
vmayoral Apr 5, 2019
990ec9c
Fix trajectory_processing tests
ahcorde Apr 25, 2019
3abb84f
moveit_core trajectory processing - library as SHARED
ahcorde May 24, 2019
ae8a43d
Fixing trajectory_processing tests
ahcorde Apr 26, 2019
cadbebd
Merge branch 'master' into trajectory-processing
ahcorde May 24, 2019
89c29ce
moveit_core trajectory_processing - fixing time
ahcorde May 24, 2019
db5104d
clang and whitespace
May 31, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 1 addition & 2 deletions moveit_core/robot_state/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -27,7 +27,6 @@ ament_target_dependencies(${MOVEIT_LIB_NAME}
urdf
tf2_geometry_msgs
)

install(TARGETS ${MOVEIT_LIB_NAME}
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
Expand All @@ -43,7 +42,7 @@ if(BUILD_TESTING)
include_directories(${moveit_resources_INCLUDE_DIRS})

if(WIN32)
# set(append_library_dirs "$<TARGET_FILE_DIR:${PROJECT_NAME}>;$<TARGET_FILE_DIR:${PROJECT_NAME}_TestPlugins1>")
# TODO add windows paths
else()
set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils")
endif()
Expand Down
55 changes: 43 additions & 12 deletions moveit_core/trajectory_processing/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,27 +1,58 @@
set(MOVEIT_LIB_NAME moveit_trajectory_processing)

add_library(${MOVEIT_LIB_NAME}
add_library(${MOVEIT_LIB_NAME} SHARED
src/iterative_time_parameterization.cpp
src/iterative_spline_parameterization.cpp
src/trajectory_tools.cpp
src/time_optimal_trajectory_generation.cpp
)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_state moveit_robot_trajectory ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_robot_state
moveit_robot_trajectory
rclcpp
rmw_implementation
urdf
urdfdom
urdfdom_headers
visualization_msgs
Boost)

install(TARGETS ${MOVEIT_LIB_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION})
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib)

install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
install(DIRECTORY include/ DESTINATION include)

if(CATKIN_ENABLE_TESTING)
if(BUILD_TESTING)
find_package(moveit_resources REQUIRED)
include_directories(${moveit_resources_INCLUDE_DIRS})
catkin_add_gtest(test_time_parameterization test/test_time_parameterization.cpp)
target_link_libraries(test_time_parameterization moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
catkin_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp)
target_link_libraries(test_time_optimal_trajectory_generation ${catkin_LIBRARIES} ${console_bridge_LIBRARIES} ${MOVEIT_LIB_NAME})
find_package(resource_retriever REQUIRED)

if(WIN32)
# TODO add windows paths
# set(append_library_dirs "$<TARGET_FILE_DIR:${PROJECT_NAME}>;$<TARGET_FILE_DIR:${PROJECT_NAME}_TestPlugins1>")
else()
set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_trajectory;${CMAKE_CURRENT_BINARY_DIR}/../utils")
Copy link
Contributor

@mlautman mlautman May 29, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Hardcoding relative paths is a bad idea.

I would recommend that for every library in this package that is used else ware you set a <lib_name>_lib_dir environment variable that can then be used by the tests

A good example is the rcl_lib_dir I have taken the time to show how it is used below:

/home/mike/ws_ros2/src/ros2/rcl/rcl/CMakeLists.txt:
   78  
   79: # rcl_lib_dir is passed as APPEND_LIBRARY_DIRS for each ament_add_gtest call so
   80  # the librcl that they link against is on the library path.
   81  # This is especially important on Windows.
   82  # This is overwritten each loop, but which one it points to doesn't really matter.
   83: set(rcl_lib_dir "$<TARGET_FILE_DIR:${PROJECT_NAME}>")
   84  

/home/mike/ws_ros2/src/ros2/rcl/rcl/test/CMakeLists.txt:
   16  
   17: set(extra_lib_dirs "${rcl_lib_dir}")
   18  
   
/home/mike/ws_ros2/src/ros2/rcl/rcl/test/CMakeLists.txt:
   34    rcl_add_custom_gtest(test_client${target_suffix}
   35      SRCS rcl/test_client.cpp
   36:     INCLUDE_DIRS ${osrf_testing_tools_cpp_INCLUDE_DIRS}
   37      ENV ${rmw_implementation_env_var}
   38      APPEND_LIBRARY_DIRS ${extra_lib_dirs}

The example uses rcl_add_custom_gtest but it works just as well with ament_add_gtest

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If you don't mind I prefer to merge this. And then I will fix them all together. Because we have other merged that use this style. I can open an issue to avoid forgetting it.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

#91

endif()

ament_add_gtest(test_time_parameterization test/test_time_parameterization.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")

target_include_directories(test_time_parameterization PUBLIC
${geometric_shapes_INCLUDE_DIRS}
)

target_link_libraries(test_time_parameterization
moveit_test_utils
moveit_robot_trajectory
moveit_trajectory_processing
${urdfdom_LIBRARIES}
${srdfdom_LIBRARIES}
${urdfdom_headers_LIBRARIES}
${MOVEIT_LIB_NAME}
${geometric_shapes_LIBRARIES}
resource_retriever::resource_retriever
)
endif()
3 changes: 2 additions & 1 deletion ...ctory_processing/include/moveit/trajectory_processing/iterative_spline_parameterization.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,10 +38,11 @@
#ifndef MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__
#define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_SPLINE_PARAMETERIZATION__

#include <trajectory_msgs/JointTrajectory.h>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <moveit_msgs/msg/joint_limits.hpp>
#include <moveit_msgs/msg/robot_state.hpp>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include "rclcpp/rclcpp.hpp"

namespace trajectory_processing
{
Expand Down
3 changes: 2 additions & 1 deletion ...jectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,11 @@
#ifndef MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_
#define MOVEIT_TRAJECTORY_PROCESSING_ITERATIVE_PARABOLIC_SMOOTHER_

#include <trajectory_msgs/JointTrajectory.h>
#include <trajectory_msgs/msg/joint_trajectory.hpp>
#include <moveit_msgs/msg/joint_limits.hpp>
#include <moveit_msgs/msg/robot_state.hpp>
#include <moveit/robot_trajectory/robot_trajectory.h>
#include "rclcpp/rclcpp.hpp"

namespace trajectory_processing
{
Expand Down
77 changes: 43 additions & 34 deletions moveit_core/trajectory_processing/src/iterative_spline_parameterization.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,9 @@ static const double ALIMIT = 1.0; // default if not specified in model

namespace trajectory_processing
{
rclcpp::Logger LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION =
rclcpp::get_logger("moveit").get_child("trajectory_processing.iterative_spline_parameterization");

static void fit_cubic_spline(const int n, const double dt[], const double x[], double x1[], double x2[]);
static void adjust_two_positions(const int n, const double dt[], double x[], double x1[], double x2[],
const double x2_i, const double x2_f);
Expand Down Expand Up @@ -90,8 +93,8 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT
const robot_model::JointModelGroup* group = trajectory.getGroup();
if (!group)
{
ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization", "It looks like the planner did not set "
"the group the plan was computed for");
RCLCPP_ERROR(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION, "It looks like the planner did not set "
"the group the plan was computed for");
return false;
}
const robot_model::RobotModel& rmodel = group->getParentModel();
Expand All @@ -106,25 +109,31 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT
if (max_velocity_scaling_factor > 0.0 && max_velocity_scaling_factor <= 1.0)
velocity_scaling_factor = max_velocity_scaling_factor;
else if (max_velocity_scaling_factor == 0.0)
ROS_DEBUG_NAMED("trajectory_processing.iterative_spline_parameterization",
"A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
velocity_scaling_factor);
{
RCLCPP_DEBUG(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION,
"A max_velocity_scaling_factor of 0.0 was specified, defaulting to %f instead.",
velocity_scaling_factor);
}
else
ROS_WARN_NAMED("trajectory_processing.iterative_spline_parameterization",
"Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
max_velocity_scaling_factor, velocity_scaling_factor);

{
RCLCPP_WARN(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION,
"Invalid max_velocity_scaling_factor %f specified, defaulting to %f instead.",
max_velocity_scaling_factor, velocity_scaling_factor);
}
if (max_acceleration_scaling_factor > 0.0 && max_acceleration_scaling_factor <= 1.0)
acceleration_scaling_factor = max_acceleration_scaling_factor;
else if (max_acceleration_scaling_factor == 0.0)
ROS_DEBUG_NAMED("trajectory_processing.iterative_spline_parameterization",
"A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
acceleration_scaling_factor);
{
RCLCPP_DEBUG(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION,
"A max_acceleration_scaling_factor of 0.0 was specified, defaulting to %f instead.",
acceleration_scaling_factor);
}
else
ROS_WARN_NAMED("trajectory_processing.iterative_spline_parameterization",
"Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
max_acceleration_scaling_factor, acceleration_scaling_factor);

{
RCLCPP_WARN(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION,
"Invalid max_acceleration_scaling_factor %f specified, defaulting to %f instead.",
max_acceleration_scaling_factor, acceleration_scaling_factor);
}
// No wrapped angles.
trajectory.unwind();

Expand Down Expand Up @@ -225,55 +234,55 @@ bool IterativeSplineParameterization::computeTimeStamps(robot_trajectory::RobotT
// Error out if bounds don't make sense
if (t2[j].max_velocity_ <= 0.0 || t2[j].max_acceleration_ <= 0.0)
{
ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
"Joint %d max velocity %f and max acceleration %f must be greater than zero "
"or a solution won't be found.\n",
j, t2[j].max_velocity_, t2[j].max_acceleration_);
RCLCPP_ERROR(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION,
"Joint %d max velocity %f and max acceleration %f must be greater than zero "
"or a solution won't be found.\n",
j, t2[j].max_velocity_, t2[j].max_acceleration_);
return false;
}
if (t2[j].min_velocity_ >= 0.0 || t2[j].min_acceleration_ >= 0.0)
{
ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
"Joint %d min velocity %f and min acceleration %f must be less than zero "
"or a solution won't be found.\n",
j, t2[j].min_velocity_, t2[j].min_acceleration_);
RCLCPP_ERROR(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION,
"Joint %d min velocity %f and min acceleration %f must be less than zero "
"or a solution won't be found.\n",
j, t2[j].min_velocity_, t2[j].min_acceleration_);
return false;
}
}

// Error check
if (num_points < 4)
{
ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
"number of waypoints %d, needs to be greater than 3.\n", num_points);
RCLCPP_ERROR(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION, "number of waypoints %d, needs to be greater than 3.\n",
num_points);
return false;
}
for (unsigned int j = 0; j < num_joints; j++)
{
if (t2[j].velocities_[0] > t2[j].max_velocity_ || t2[j].velocities_[0] < t2[j].min_velocity_)
{
ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization", "Initial velocity %f out of bounds\n",
t2[j].velocities_[0]);
RCLCPP_ERROR(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION, "Initial velocity %f out of bounds\n",
t2[j].velocities_[0]);
return false;
}
else if (t2[j].velocities_[num_points - 1] > t2[j].max_velocity_ ||
t2[j].velocities_[num_points - 1] < t2[j].min_velocity_)
{
ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization", "Final velocity %f out of bounds\n",
t2[j].velocities_[num_points - 1]);
RCLCPP_ERROR(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION, "Final velocity %f out of bounds\n",
t2[j].velocities_[num_points - 1]);
return false;
}
else if (t2[j].accelerations_[0] > t2[j].max_acceleration_ || t2[j].accelerations_[0] < t2[j].min_acceleration_)
{
ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
"Initial acceleration %f out of bounds\n", t2[j].accelerations_[0]);
RCLCPP_ERROR(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION, "Initial acceleration %f out of bounds\n",
t2[j].accelerations_[0]);
return false;
}
else if (t2[j].accelerations_[num_points - 1] > t2[j].max_acceleration_ ||
t2[j].accelerations_[num_points - 1] < t2[j].min_acceleration_)
{
ROS_ERROR_NAMED("trajectory_processing.iterative_spline_parameterization",
"Final acceleration %f out of bounds\n", t2[j].accelerations_[num_points - 1]);
RCLCPP_ERROR(LOGGER_ITERATIVE_SPLINE_PARAMETERIZATION, "Final acceleration %f out of bounds\n",
t2[j].accelerations_[num_points - 1]);
return false;
}
}
Expand Down
Loading