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 1 commit
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
37 changes: 23 additions & 14 deletions moveit_core/kinematic_constraints/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,21 +5,30 @@ add_library(${MOVEIT_LIB_NAME}
src/utils.cpp)
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

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})
ament_target_dependencies(${MOVEIT_LIB_NAME}
urdf
urdfdom
urdfdom_headers
moveit_robot_model
tf2_geometry_msgs
visualization_msgs
moveit_kinematics_base
moveit_robot_state
tf2_eigen
moveit_collision_detection_fcl)

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)
find_package(moveit_resources 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})
endif()
#TODO: Review once moveit_resources has been ported
# if(BUILD_TESTING)
# find_package(ament_cmake_gtest REQUIRED)
# find_package(moveit_resources REQUIRED)
# include_directories(${moveit_resources_INCLUDE_DIRS})
#
# ament_add_gtest(test_constraints test/test_constraints.cpp)
# target_link_libraries(test_constraints moveit_test_utils ${MOVEIT_LIB_NAME})
# endif()
22 changes: 20 additions & 2 deletions ...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 @@ -44,11 +44,15 @@
#include <moveit/macros/class_forward.h>

#include <geometric_shapes/bodies.h>
#include <moveit_msgs/Constraints.h>
#include <moveit_msgs/msg/constraints.hpp>

#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
22 changes: 11 additions & 11 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 @@ -37,10 +37,10 @@
#ifndef MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_
#define MOVEIT_KINEMATIC_CONSTRAINTS_UTILS_

#include <moveit_msgs/MotionPlanRequest.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/QuaternionStamped.h>
#include <moveit_msgs/msg/motion_plan_request.hpp>
#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,7 @@ 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);
// 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