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

Port kinematic_constraints to ROS2 #42

Merged
merged 15 commits into from
Jun 25, 2019
Merged

Port kinematic_constraints to ROS2 #42

Show file tree
Hide file tree
Changes from 14 commits
Commits
Show all changes
15 commits
Select commit Hold shift + click to select a range
7cdb756
Port kinematic_constraints to ROS2
vmayoral Mar 10, 2019
0f079c9
Merge branch 'moveit2' into kinematic-constraints
vmayoral Mar 30, 2019
5ace7e8
kinematic-constraints, first steps towards unit tests
vmayoral Mar 30, 2019
8d49463
Fixing kinematic constraints tests
ahcorde Apr 26, 2019
d70f888
kinematic_constraints adding library dependencies
ahcorde May 8, 2019
beb3bca
moveit_core kinematics_constraints - addressing some issues
ahcorde May 24, 2019
61cdfba
Merge branch 'master' into kinematic-constraints
ahcorde May 24, 2019
6b1dd61
moveit_core kinematics_constraints - logger_kinematic_constraints to …
ahcorde May 29, 2019
57f271e
moveit_core kinematics_constraints - remove duplicated getMarkers
ahcorde May 29, 2019
c6aa8e6
moveit_core kinematics_constraints - fix whitespaces in CMakeLists.txt
ahcorde May 29, 2019
df09d3a
moveit_core kinematics_constraints - fix whitespaces in CMakeLists.txt
ahcorde May 29, 2019
da78630
moveit_core kinematic_constraints - Fix getMarkers header
ahcorde May 30, 2019
b3855c3
moveit_core kinematics constraints - converting boost to std
ahcorde Jun 4, 2019
4ff5652
Merge branch 'master' into kinematic-constraints
ahcorde Jun 4, 2019
f4a7b2d
Remove not necessary stamp
anasarrak Jun 6, 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
12 changes: 12 additions & 0 deletions moveit_core/collision_detection/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,19 @@ add_library(${MOVEIT_LIB_NAME} SHARED
src/world.cpp
src/world_diff.cpp
)

set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_robot_state
rclcpp
rmw_implementation
urdf
urdfdom
urdfdom_headers
visualization_msgs
tf2_eigen
geometric_shapes
)

ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_robot_state
Expand Down
3 changes: 1 addition & 2 deletions moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,6 @@
#include <moveit/collision_detection/collision_common.h>
#include <moveit/macros/class_forward.h>
#include <moveit_msgs/msg/allowed_collision_matrix.hpp>
#include <boost/function.hpp>
#include <iostream>
#include <vector>
#include <string>
Expand Down Expand Up @@ -70,7 +69,7 @@ enum Type

/** \brief Signature of predicate that decides whether a contact is allowed or not (when AllowedCollision::Type is
* CONDITIONAL) */
typedef boost::function<bool(collision_detection::Contact&)> DecideContactFn;
typedef std::function<bool(collision_detection::Contact&)> DecideContactFn;

MOVEIT_CLASS_FORWARD(AllowedCollisionMatrix);

Expand Down
58 changes: 47 additions & 11 deletions moveit_core/kinematic_constraints/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,25 +1,61 @@
set(MOVEIT_LIB_NAME moveit_kinematic_constraints)

add_library(${MOVEIT_LIB_NAME}
if(WIN32)
# 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}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl")
endif()

add_library(${MOVEIT_LIB_NAME} SHARED
src/kinematic_constraint.cpp
src/utils.cpp)
src/utils.cpp
)

set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

ament_target_dependencies(${MOVEIT_LIB_NAME}
urdf
urdfdom
urdfdom_headers
tf2_geometry_msgs
visualization_msgs
tf2_eigen
)

target_link_libraries(${MOVEIT_LIB_NAME}
moveit_robot_model moveit_kinematics_base moveit_robot_state moveit_collision_detection_fcl moveit_utils
${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
moveit_collision_detection_fcl
moveit_kinematics_base
moveit_robot_state
moveit_robot_model
)

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

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

if(CATKIN_ENABLE_TESTING)
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(moveit_resources REQUIRED)
find_package(resource_retriever REQUIRED)

include_directories(${moveit_resources_INCLUDE_DIRS})

catkin_add_gtest(test_constraints test/test_constraints.cpp)
target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME})
ament_add_gtest(test_constraints test/test_constraints.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}"
)

target_link_libraries(test_constraints
moveit_kinematic_constraints
moveit_collision_detection_fcl
moveit_robot_model
moveit_robot_state
moveit_utils
moveit_test_utils
${geometric_shapes_LIBRARIES}
resource_retriever::resource_retriever
${MOVEIT_LIB_NAME}
)
endif()
6 changes: 5 additions & 1 deletion ...it_core/kinematic_constraints/include/moveit/kinematic_constraints/kinematic_constraint.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,10 @@
#include <iostream>
#include <vector>

#include "rclcpp/clock.hpp"
#include "rclcpp/rclcpp.hpp"
#include "rclcpp/duration.hpp"

/** \brief Representation and evaluation of kinematic constraints */
namespace kinematic_constraints
{
Expand Down Expand Up @@ -814,7 +818,7 @@ class VisibilityConstraint : public KinematicConstraint
* @param [in] state The state from which to produce the markers
* @param [out] markers The marker array to which the markers will be added
*/
void getMarkers(const robot_state::RobotState& state, visualization_msgs::MarkerArray& markers) const;
void getMarkers(const robot_state::RobotState& state, visualization_msgs::msg::MarkerArray& markers) const;

bool enabled() const override;
ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override;
Expand Down
21 changes: 11 additions & 10 deletions moveit_core/kinematic_constraints/include/moveit/kinematic_constraints/utils.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,9 +38,9 @@
#define MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_

#include <moveit_msgs/msg/motion_plan_request.hpp>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <geometry_msgs/msg/point_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/quaternion_stamped.hpp>
#include <moveit/robot_state/robot_state.h>
#include <limits>

Expand Down Expand Up @@ -118,7 +118,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const robot_state::RobotS
*
* @return A full constraint message containing both constraints
*/
moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose,
double tolerance_pos = 1e-3, double tolerance_angle = 1e-2);

/**
Expand All @@ -136,7 +136,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
*
* @return A full constraint message containing both constraints
*/
moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::PoseStamped& pose,
moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name, const geometry_msgs::msg::PoseStamped& pose,
const std::vector<double>& tolerance_pos,
const std::vector<double>& tolerance_angle);

Expand All @@ -152,7 +152,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
* @return A full constraint message containing the orientation constraint
*/
moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
const geometry_msgs::QuaternionStamped& quat,
const geometry_msgs::msg::QuaternionStamped& quat,
double tolerance = 1e-2);

/**
Expand All @@ -169,8 +169,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
* @return A full constraint message containing the position constraint
*/
moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
const geometry_msgs::Point& reference_point,
const geometry_msgs::PointStamped& goal_point,
const geometry_msgs::msg::Point& reference_point,
const geometry_msgs::msg::PointStamped& goal_point,
double tolerance = 1e-3);

/**
Expand All @@ -186,7 +186,7 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
* @return A full constraint message containing the position constraint
*/
moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_name,
const geometry_msgs::PointStamped& goal_point,
const geometry_msgs::msg::PointStamped& goal_point,
double tolerance = 1e-3);

/**
Expand All @@ -200,7 +200,8 @@ moveit_msgs::msg::Constraints constructGoalConstraints(const std::string& link_n
*
* @return was the construction successful?
*/
bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints);
// TODO rework this function
// bool constructConstraints(XmlRpc::XmlRpcValue& params, moveit_msgs::msg::Constraints& constraints);
Copy link
Contributor

Choose a reason for hiding this comment

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

Add TODO with missing dependency. Was this removed because xmlrpc is not available? If so, it looks like it is used in the ros1_bridge

Copy link
Contributor

Choose a reason for hiding this comment

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

We can actually use the rclcpp parameters to fix these kind of things, or at least, the way that I'm following.

Copy link
Contributor

Choose a reason for hiding this comment

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

@v4hn spent a lot of time on this feature. Perhaps you and he can collaborate on the best path for porting this. In the mean time, any time you comment out a feature, please add comments explaining why the feature is being removed and what steps would be necessary before re-enabling the feature

}

#endif
Loading