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.

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

Port kinematics_plugin_loader to ROS2 #107

Merged
merged 4 commits into from
Dec 6, 2019
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
6 changes: 3 additions & 3 deletions moveit_ros/planning/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -29,7 +29,7 @@ find_package(ament_index_cpp REQUIRED)

set(THIS_PACKAGE_INCLUDE_DIRS
rdf_loader/include
# kinematics_plugin_loader/include
kinematics_plugin_loader/include
# robot_model_loader/include
# constraint_sampler_manager_loader/include
# planning_pipeline/include
Expand All @@ -41,7 +41,7 @@ set(THIS_PACKAGE_INCLUDE_DIRS

set(libraries
moveit_rdf_loader
# moveit_kinematics_plugin_loader
moveit_kinematics_plugin_loader
# moveit_robot_model_loader
# moveit_constraint_sampler_manager_loader
# moveit_planning_pipeline
Expand Down Expand Up @@ -72,7 +72,7 @@ include_directories(SYSTEM

add_subdirectory(rdf_loader)
# add_subdirectory(collision_plugin_loader)
# add_subdirectory(kinematics_plugin_loader)
add_subdirectory(kinematics_plugin_loader)
# add_subdirectory(robot_model_loader)
# add_subdirectory(constraint_sampler_manager_loader)
# add_subdirectory(planning_pipeline)
Expand Down
19 changes: 14 additions & 5 deletions moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -2,10 +2,19 @@ set(MOVEIT_LIB_NAME moveit_kinematics_plugin_loader)

add_library(${MOVEIT_LIB_NAME} src/kinematics_plugin_loader.cpp)
henningkayser marked this conversation as resolved.
Show resolved Hide resolved
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
target_link_libraries(${MOVEIT_LIB_NAME} moveit_rdf_loader ${catkin_LIBRARIES})
ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_rdf_loader
JafarAbdi marked this conversation as resolved.
Show resolved Hide resolved
rclcpp
urdf
class_loader
ament_index_cpp
moveit_core
Boost)


install(TARGETS ${MOVEIT_LIB_NAME}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION})
install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)

install(DIRECTORY include/ DESTINATION include)
Original file line number Diff line number Diff line change
Expand Up @@ -50,27 +50,29 @@ class KinematicsPluginLoader
{
public:
/** \brief Load the kinematics solvers based on information on the
ROS parameter server. Take as optional argument the name of the
ROS parameter server. Take node as an argument and as optional argument the name of the
ROS parameter under which the robot description can be
found. This is passed to the kinematics solver initialization as
well as used to read the SRDF document when needed. */
KinematicsPluginLoader(const std::string& robot_description = "robot_description",
KinematicsPluginLoader(const rclcpp::Node::SharedPtr& node,
JafarAbdi marked this conversation as resolved.
Show resolved Hide resolved
const std::string& robot_description = "robot_description",
double default_search_resolution = 0.0)
: robot_description_(robot_description), default_search_resolution_(default_search_resolution)
: node_(node), robot_description_(robot_description), default_search_resolution_(default_search_resolution)
{
}

/** \brief Use a default kinematics solver (\e solver_plugin) for
all the groups in the robot model. The default timeout for the
solver is \e solve_timeout and the default number of IK attempts
is \e ik_attempts. Take as optional argument the name of the ROS
is \e ik_attempts. Take node as an argument and as optional argument the name of the ROS
parameter under which the robot description can be found. This
is passed to the kinematics solver initialization as well as
used to read the SRDF document when needed. */
KinematicsPluginLoader(const std::string& solver_plugin, double solve_timeout, unsigned int /*ik_attempts*/,
KinematicsPluginLoader(const rclcpp::Node::SharedPtr& node, const std::string& solver_plugin, double solve_timeout,
const std::string& robot_description = "robot_description",
double default_search_resolution = 0.0)
: robot_description_(robot_description)
: node_(node)
, robot_description_(robot_description)
, default_search_resolution_(default_search_resolution)
, default_solver_plugin_(solver_plugin)
, default_solver_timeout_(solve_timeout)
Expand Down Expand Up @@ -100,6 +102,7 @@ class KinematicsPluginLoader
void status() const;

private:
const rclcpp::Node::SharedPtr node_;
std::string robot_description_;
double default_search_resolution_;

Expand Down
Loading