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

Jump to bottom

Port planning_scene to ROS2 #43

Merged
merged 11 commits into from
Jun 12, 2019
Merged

Port planning_scene to ROS2 #43

Show file tree
Hide file tree
Changes from 10 commits
Commits
Show all changes
11 commits
Select commit Hold shift + click to select a range
1445fe7
Port planning_scene to ROS2
vmayoral Mar 10, 2019
3181e29
planning_scene, adapt logger to https://github.com/ros-planning/movei…
vmayoral Mar 30, 2019
e6f6082
planning_scene - applying clang format 3.9
ahcorde May 17, 2019
d5bd6ca
planning_scene smart pointer world passed by value
ahcorde May 17, 2019
ba1e600
Merge branch 'master' into planning-scene
May 17, 2019
ca02302
clang
May 21, 2019
acd617d
moveit_core planning_scene - restoring constructor headers
ahcorde May 21, 2019
6d35b17
moveit_core planning_scene - Fixing Cmakelists.txt and including test…
ahcorde May 22, 2019
15ba426
move_core planning_scene - Logger inside the namespace
ahcorde May 22, 2019
9fa297f
moveit_core planning_scene - fixing CMakeLists.txt
ahcorde May 24, 2019
01f9e4f
clang and whitespace
Jun 1, 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
45 changes: 35 additions & 10 deletions moveit_core/planning_scene/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,30 +1,55 @@
set(MOVEIT_LIB_NAME moveit_planning_scene)

add_library(${MOVEIT_LIB_NAME} src/planning_scene.cpp)
add_library(${MOVEIT_LIB_NAME} SHARED src/planning_scene.cpp)
#TODO: Fix the versioning
set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")

target_include_directories(${MOVEIT_LIB_NAME} PUBLIC
${octomap_msgs_INCLUDE_DIRS}
${octomap_INCLUDE_DIRS}
)

target_link_libraries(${MOVEIT_LIB_NAME}
moveit_robot_model
moveit_robot_state
moveit_exceptions
moveit_transforms
moveit_collision_detection_fcl
moveit_collision_detection
moveit_kinematic_constraints
moveit_robot_trajectory
moveit_trajectory_processing
${LIBOCTOMAP_LIBRARIES} ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})

add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
${LIBOCTOMAP_LIBRARIES}
${rclcpp_LIBRARIES}
${rmw_implementation_LIBRARIES}
${urdfdom_LIBRARIES}
${urdfdom_headers_LIBRARIES}
${Boost_LIBRARIES}
${octomap_msgs_LIBRARIES}
${OCTOMAP_LIBRARIES}
)

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

if(CATKIN_ENABLE_TESTING)
if(BUILD_TESTING)
find_package(moveit_resources REQUIRED)
find_package(ament_cmake_gtest REQUIRED)
include_directories(${moveit_resources_INCLUDE_DIRS})

catkin_add_gtest(test_planning_scene test/test_planning_scene.cpp)
target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME})
if(UNIX OR APPLE)
set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_BINARY_DIR}/../utils:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection")
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 should work 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()

ament_add_gtest(test_planning_scene test/test_planning_scene.cpp
APPEND_LIBRARY_DIRS "${append_library_dirs}")
target_link_libraries(test_planning_scene
${MOVEIT_LIB_NAME}
${srdfdom_LIBRARIES}
${urdf_LIBRARIES}
${geometric_shapes_LIBRARIES}
${OCTOMAP_LIBRARIES}
)
endif()
52 changes: 27 additions & 25 deletions moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,11 +51,12 @@
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/msg/constraints.hpp>
#include <moveit_msgs/msg/planning_scene_components.hpp>
#include <octomap_msgs/OctomapWithPose.h>
#include <octomap_msgs/msg/octomap_with_pose.hpp>
#include <boost/noncopyable.hpp>
#include <boost/function.hpp>
#include <boost/concept_check.hpp>
#include <memory>
#include "rclcpp/rclcpp.hpp"

/** \brief This namespace includes the central class for representing planning contexts */
namespace planning_scene
Expand All @@ -76,10 +77,10 @@ typedef boost::function<bool(const robot_state::RobotState&, bool)> StateFeasibi
typedef boost::function<bool(const robot_state::RobotState&, const robot_state::RobotState&, bool)> MotionFeasibilityFn;

/** \brief A map from object names (e.g., attached bodies, collision objects) to their colors */
typedef std::map<std::string, std_msgs::ColorRGBA> ObjectColorMap;
typedef std::map<std::string, std_msgs::msg::ColorRGBA> ObjectColorMap;

/** \brief A map from object names (e.g., attached bodies, collision objects) to their types */
typedef std::map<std::string, object_recognition_msgs::ObjectType> ObjectTypeMap;
typedef std::map<std::string, object_recognition_msgs::msg::ObjectType> ObjectTypeMap;

/** \brief This class maintains the representation of the
environment as seen by a planning instance. The environment
Expand All @@ -88,15 +89,13 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
{
public:
/** \brief construct using an existing RobotModel */
PlanningScene(
const robot_model::RobotModelConstPtr& robot_model,
const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));
PlanningScene(const robot_model::RobotModelConstPtr& robot_model,
const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));

/** \brief construct using a urdf and srdf.
* A RobotModel for the PlanningScene will be created using the urdf and srdf. */
PlanningScene(
const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));
PlanningScene(const urdf::ModelInterfaceSharedPtr& urdf_model, const srdf::ModelConstSharedPtr& srdf_model,
const collision_detection::WorldPtr& world = collision_detection::WorldPtr(new collision_detection::World()));

static const std::string OCTOMAP_NS;
static const std::string DEFAULT_SCENE_NAME;
Expand Down Expand Up @@ -705,7 +704,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from

/** \brief Construct a message (\e scene) with the data requested in \e comp. If all options in \e comp are filled,
this will be a complete planning scene message */
void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene, const moveit_msgs::msg::PlanningSceneComponents& comp) const;
void getPlanningSceneMsg(moveit_msgs::msg::PlanningScene& scene,
const moveit_msgs::msg::PlanningSceneComponents& comp) const;

/** \brief Construct a message (\e collision_object) with the collision object data from the planning_scene for the
* requested object*/
Expand All @@ -722,10 +722,11 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from

/** \brief Construct a vector of messages (\e attached_collision_objects) with the attached collision object data for
* all objects in planning_scene */
void getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objs) const;
void
getAttachedCollisionObjectMsgs(std::vector<moveit_msgs::msg::AttachedCollisionObject>& attached_collision_objs) const;

/** \brief Construct a message (\e octomap) with the octomap data from the planning_scene */
bool getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const;
bool getOctomapMsg(octomap_msgs::msg::OctomapWithPose& octomap) const;

/** \brief Construct a vector of messages (\e object_colors) with the colors of the objects from the planning_scene */
void getObjectColorMsgs(std::vector<moveit_msgs::msg::ObjectColor>& object_colors) const;
Expand All @@ -750,8 +751,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from

bool processPlanningSceneWorldMsg(const moveit_msgs::msg::PlanningSceneWorld& world);

void processOctomapMsg(const octomap_msgs::OctomapWithPose& map);
void processOctomapMsg(const octomap_msgs::Octomap& map);
void processOctomapMsg(const octomap_msgs::msg::OctomapWithPose& map);
void processOctomapMsg(const octomap_msgs::msg::Octomap& map);
void processOctomapPtr(const std::shared_ptr<const octomap::OcTree>& octree, const Eigen::Isometry3d& t);

/**
Expand All @@ -775,15 +776,15 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from

bool hasObjectColor(const std::string& id) const;

const std_msgs::ColorRGBA& getObjectColor(const std::string& id) const;
void setObjectColor(const std::string& id, const std_msgs::ColorRGBA& color);
const std_msgs::msg::ColorRGBA& getObjectColor(const std::string& id) const;
void setObjectColor(const std::string& id, const std_msgs::msg::ColorRGBA& color);
void removeObjectColor(const std::string& id);
void getKnownObjectColors(ObjectColorMap& kc) const;

bool hasObjectType(const std::string& id) const;

const object_recognition_msgs::ObjectType& getObjectType(const std::string& id) const;
void setObjectType(const std::string& id, const object_recognition_msgs::ObjectType& type);
const object_recognition_msgs::msg::ObjectType& getObjectType(const std::string& id) const;
void setObjectType(const std::string& id, const object_recognition_msgs::msg::ObjectType& type);
void removeObjectType(const std::string& id);
void getKnownObjectTypes(ObjectTypeMap& kc) const;

Expand Down Expand Up @@ -855,7 +856,8 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
const kinematic_constraints::KinematicConstraintSet& constr, bool verbose = false) const;

/** \brief Check if a given state is valid. This means checking for collisions and feasibility */
bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "", bool verbose = false) const;
bool isStateValid(const moveit_msgs::msg::RobotState& state, const std::string& group = "",
bool verbose = false) const;

/** \brief Check if a given state is valid. This means checking for collisions and feasibility */
bool isStateValid(const robot_state::RobotState& state, const std::string& group = "", bool verbose = false) const;
Expand Down Expand Up @@ -891,9 +893,9 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
* constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the
* passed in trajectory. */
bool isPathValid(const moveit_msgs::msg::RobotState& start_state, const moveit_msgs::msg::RobotTrajectory& trajectory,
const moveit_msgs::msg::Constraints& path_constraints, const moveit_msgs::msg::Constraints& goal_constraints,
const std::string& group = "", bool verbose = false,
std::vector<std::size_t>* invalid_index = NULL) const;
const moveit_msgs::msg::Constraints& path_constraints,
const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "",
bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;

/** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and
* constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the
Expand All @@ -915,9 +917,9 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
* constraint satisfaction). It is also checked that the goal constraints are satisfied by the last state on the
* passed in trajectory. */
bool isPathValid(const robot_trajectory::RobotTrajectory& trajectory,
const moveit_msgs::msg::Constraints& path_constraints, const moveit_msgs::msg::Constraints& goal_constraints,
const std::string& group = "", bool verbose = false,
std::vector<std::size_t>* invalid_index = NULL) const;
const moveit_msgs::msg::Constraints& path_constraints,
const moveit_msgs::msg::Constraints& goal_constraints, const std::string& group = "",
bool verbose = false, std::vector<std::size_t>* invalid_index = NULL) const;

/** \brief Check if a given path is valid. Each state is checked for validity (collision avoidance, feasibility and
* constraint satisfaction). */
Expand Down
Loading