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 7 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
54 changes: 44 additions & 10 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,59 @@
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")
Copy link
Contributor

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.

endif()

add_library(${MOVEIT_LIB_NAME} SHARED
src/kinematic_constraint.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
mlautman marked this conversation as resolved.
Show resolved Hide resolved
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()
20 changes: 19 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,20 @@ 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;

/**
* \brief Adds markers associated with the visibility cone, sensor
* and target to the visualization array
*
* The visibility cone and two arrows - a blue array that issues
* from the sensor_view_direction of the sensor, and a red arrow the
* issues along the Z axis of the the target frame.
*
* @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::msg::MarkerArray& markers);
mlautman marked this conversation as resolved.
Show resolved Hide resolved

bool enabled() const override;
ConstraintEvaluationResult decide(const robot_state::RobotState& state, bool verbose = false) const override;
Expand Down Expand Up @@ -846,6 +863,7 @@ class VisibilityConstraint : public KinematicConstraint
double target_radius_; /**< \brief Storage for the target radius */
double max_view_angle_; /**< \brief Storage for the max view angle */
double max_range_angle_; /**< \brief Storage for the max range angle */
rclcpp::Clock clock_ros_; /**< \brief ros2 clock for the time */
};

MOVEIT_CLASS_FORWARD(KinematicConstraintSet);
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