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 6 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
28 changes: 14 additions & 14 deletions moveit_core/planning_scene/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,7 +1,8 @@
set(MOVEIT_LIB_NAME moveit_planning_scene)

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

Choose a reason for hiding this comment

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

This was added in moveit/moveit#273. Maybe @v4hn can confirm this but perhaps we can take the same approach as the urdf_parser


target_link_libraries(${MOVEIT_LIB_NAME}
moveit_robot_model
Expand All @@ -12,19 +13,18 @@ target_link_libraries(${MOVEIT_LIB_NAME}
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})

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})

if(CATKIN_ENABLE_TESTING)
find_package(moveit_resources REQUIRED)
include_directories(${moveit_resources_INCLUDE_DIRS})
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
install(DIRECTORY include/ DESTINATION include)

catkin_add_gtest(test_planning_scene test/test_planning_scene.cpp)
target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME})
endif()
#TODO: Fix this so it can be built with colcon
# if(BUILD_TESTING)
Copy link
Member

Choose a reason for hiding this comment

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

re-enable tests please

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Tests failing. Let's try to solve AcutronicRobotics#13 first please. We should be able to re-enable all of them afterwards.

Copy link
Member

Choose a reason for hiding this comment

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

You all fixed this somewhere in AcutronicRobotics#71, is that correct?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Yes however they should be submitted here (and we should rebase this code as well). @ahcorde started doing it iteratively while we review the changes.

Copy link
Contributor

Choose a reason for hiding this comment

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

+1 to restoring tests before merge

# find_package(moveit_resources REQUIRED)
# include_directories(${moveit_resources_INCLUDE_DIRS})
#
# ament_add_gtest(test_planning_scene test/test_planning_scene.cpp)
# target_link_libraries(test_planning_scene ${MOVEIT_LIB_NAME})
# 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,
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,
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