Port robot_state to ROS2

Authored by @anasarrak

moveit_core/robot_state/CMakeLists.txt

@@ -1,41 +1,36 @@
 set(MOVEIT_LIB_NAME moveit_robot_state)
 
-if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
-  # clang is picky about typeid(*solver)
-  add_compile_options(-Wno-potentially-evaluated-expression)
-endif()
-
 add_library(${MOVEIT_LIB_NAME}
   src/attached_body.cpp
   src/conversions.cpp
   src/robot_state.cpp
 )
-set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}")
+set_target_properties(${MOVEIT_LIB_NAME} PROPERTIES VERSION ${${PROJECT_NAME}_VERSION})
 
-target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_kinematics_base moveit_transforms ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
+target_link_libraries(${MOVEIT_LIB_NAME} moveit_robot_model moveit_kinematics_base moveit_transforms ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})
 
-add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
+# add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
 
 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)
 
 # Unit tests
 if(CATKIN_ENABLE_TESTING)
   find_package(moveit_resources REQUIRED)
   include_directories(${moveit_resources_INCLUDE_DIRS})
 
   catkin_add_gtest(test_robot_state test/robot_state_test.cpp)
-  target_link_libraries(test_robot_state moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
+  target_link_libraries(test_robot_state moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
 
   # As an executable, this benchmark is not run as a test by default
   add_executable(robot_state_benchmark test/robot_state_benchmark.cpp)
-  target_link_libraries(robot_state_benchmark moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME} ${GTEST_LIBRARIES})
+  target_link_libraries(robot_state_benchmark moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME} ${GTEST_LIBRARIES})
 
   catkin_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp)
-  target_link_libraries(test_robot_state_complex moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
+  target_link_libraries(test_robot_state_complex moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
 
   catkin_add_gtest(test_aabb test/test_aabb.cpp)
-  target_link_libraries(test_aabb moveit_test_utils ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
+  target_link_libraries(test_aabb moveit_test_utils ${rclcpp_LIBRARIES} ${rmw_implementation_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${MOVEIT_LIB_NAME})
 endif()

moveit_core/robot_state/include/moveit/robot_state/attached_body.h

@@ -40,7 +40,7 @@
 #include <moveit/robot_model/link_model.h>
 #include <eigen_stl_containers/eigen_stl_containers.h>
 #include <boost/function.hpp>
-#include <trajectory_msgs/JointTrajectory.h>
+#include <trajectory_msgs/msg/joint_trajectory.hpp>
 #include <set>
 
 namespace moveit
@@ -62,7 +62,7 @@ class AttachedBody
      object is specified by \e touch_links. */
   AttachedBody(const LinkModel* link, const std::string& id, const std::vector<shapes::ShapeConstPtr>& shapes,
                const EigenSTL::vector_Isometry3d& attach_trans, const std::set<std::string>& touch_links,
-               const trajectory_msgs::JointTrajectory& attach_posture);
+               const trajectory_msgs::msg::JointTrajectory& attach_posture);
 
   ~AttachedBody();
 
@@ -99,7 +99,7 @@ class AttachedBody
   /** \brief Return the posture that is necessary for the object to be released, (if any). This is useful for example
      when storing
       the configuration of a gripper holding an object */
-  const trajectory_msgs::JointTrajectory& getDetachPosture() const
+  const trajectory_msgs::msg::JointTrajectory& getDetachPosture() const
   {
     return detach_posture_;
   }
@@ -147,7 +147,7 @@ class AttachedBody
 
   /** \brief Posture of links for releasing the object (if any). This is useful for example when storing
       the configuration of a gripper holding an object */
-  trajectory_msgs::JointTrajectory detach_posture_;
+  trajectory_msgs::msg::JointTrajectory detach_posture_;
 
   /** \brief The global transforms for these attached bodies (computed by forward kinematics) */
   EigenSTL::vector_Isometry3d global_collision_body_transforms_;

moveit_core/robot_state/include/moveit/robot_state/conversions.h

@@ -39,8 +39,8 @@
 
 #include <moveit/robot_state/robot_state.h>
 #include <moveit/transforms/transforms.h>
-#include <moveit_msgs/RobotState.h>
-#include <moveit_msgs/RobotTrajectory.h>
+#include <moveit_msgs/msg/robot_state.hpp>
+#include <moveit_msgs/msg/robot_trajectory.hpp>
 
 namespace moveit
 {
@@ -52,7 +52,7 @@ namespace core
  * @param state The resultant MoveIt! robot state
  * @return True if successful, false if failed for any reason
  */
-bool jointStateToRobotState(const sensor_msgs::JointState& joint_state, RobotState& state);
+bool jointStateToRobotState(const sensor_msgs::msg::JointState& joint_state, RobotState& state);
 
 /**
  * @brief Convert a robot state msg (with accompanying extra transforms) to a MoveIt! robot state
@@ -97,7 +97,7 @@ void attachedBodiesToAttachedCollisionObjectMsgs(
  * @param state The input MoveIt! robot state object
  * @param robot_state The resultant JointState message
  */
-void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState& joint_state);
+void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::msg::JointState& joint_state);
 
 /**
  * @brief Convert a joint trajectory point to a MoveIt! robot state
@@ -106,7 +106,7 @@ void robotStateToJointStateMsg(const RobotState& state, sensor_msgs::JointState&
  * @param state The resultant MoveIt! robot state
  * @return True if successful, false if failed for any reason
  */
-bool jointTrajPointToRobotState(const trajectory_msgs::JointTrajectory& trajectory, std::size_t point_id,
+bool jointTrajPointToRobotState(const trajectory_msgs::msg::JointTrajectory& trajectory, std::size_t point_id,
                                 RobotState& state);
 
 /**

moveit_core/robot_state/include/moveit/robot_state/log.h

@@ -0,0 +1,53 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2019, Acutronic Robotics AG
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *   * Neither the name of Willow Garage nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+/* Author: Víctor Mayoral Vilches */
+
+#ifndef MOVEIT_CORE_ROBOT_STATE_LOG_
+#define MOVEIT_CORE_ROBOT_STATE_LOG_
+
+#include "rclcpp/rclcpp.hpp"
+
+namespace moveit
+{
+namespace core
+{
+
+// Logger
+rclcpp::Logger logger_robot_state = rclcpp::get_logger("robot_state");
+
+};
+};
+
+#endif