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

Robot state port #80

Merged
merged 13 commits into from
May 21, 2019
Merged

Robot state port #80

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
7599e6b
Port robot_state to ROS2
ahcorde May 16, 2019
ab2c728
Fixing Logger
ahcorde May 16, 2019
11701fe
robot_state - fixing tests
ahcorde May 16, 2019
61cf614
robot_state - Fixing CMakeLists.txt
ahcorde May 16, 2019
3fc4471
robot_state - fixing wall timer
ahcorde May 16, 2019
47052c9
robot_state applying clang format
ahcorde May 16, 2019
7b5dd79
robot_state, remove log.h
vmayoral Mar 16, 2019
bab3a52
Update moveit_core/robot_state/CMakeLists.txt
ahcorde May 17, 2019
4d3c4cb
Update moveit_core/robot_state/CMakeLists.txt
ahcorde May 17, 2019
3dde080
robot_state restoring clang warnings compiler option
ahcorde May 17, 2019
f1f8f08
robot_state fixing indentation CMakelists.txt
ahcorde May 17, 2019
0525925
Update moveit_core/robot_state/src/conversions.cpp
ahcorde May 20, 2019
a5a795f
tabs to spaces
May 21, 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
148 changes: 123 additions & 25 deletions moveit_core/robot_state/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,41 +1,139 @@
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()
ahcorde marked this conversation as resolved.
Show resolved Hide resolved

add_library(${MOVEIT_LIB_NAME}
add_library(${MOVEIT_LIB_NAME} SHARED
src/attached_body.cpp
src/conversions.cpp
src/robot_state.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_transforms ${catkin_LIBRARIES} ${urdfdom_LIBRARIES} ${urdfdom_headers_LIBRARIES} ${Boost_LIBRARIES})

add_dependencies(${MOVEIT_LIB_NAME} ${catkin_EXPORTED_TARGETS})
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
${urdfdom_LIBRARIES}
${geometric_shapes_LIBRARIES}
${urdfdom_headers_LIBRARIES}
${Boost_LIBRARIES})
ahcorde marked this conversation as resolved.
Show resolved Hide resolved
ament_target_dependencies(${MOVEIT_LIB_NAME}
moveit_robot_model
moveit_kinematics_base
moveit_transforms
urdf
tf2_geometry_msgs
)

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
ahcorde marked this conversation as resolved.
Show resolved Hide resolved
if(BUILD_TESTING)
find_package(ament_cmake_gtest REQUIRED)
find_package(moveit_resources REQUIRED)
ahcorde marked this conversation as resolved.
Show resolved Hide resolved
find_package(tf2_geometry_msgs REQUIRED)
find_package(resource_retriever REQUIRED)

# 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})
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")
endif()

ament_add_gtest(test_robot_state test/robot_state_test.cpp
ahcorde marked this conversation as resolved.
Show resolved Hide resolved
APPEND_LIBRARY_DIRS "${append_library_dirs}")

target_include_directories(test_robot_state PUBLIC
${tf2_geometry_msgs_INCLUDE_DIRS}
${urdf_INCLUDE_DIRS}
${visualization_msgs_INCLUDE_DIRS}
${geometric_shapes_INCLUDE_DIRS}
${srdfdom_INCLUDE_DIRS}
)

target_link_libraries(test_robot_state
moveit_test_utils
moveit_utils
moveit_exceptions
${tf2_geometry_msgs_LIBRARIES}
${srdfdom_LIBRARIES}
${MOVEIT_LIB_NAME}
${moveit_profiler_LIBRARIES}
${urdf_LIBRARIES}
${visualization_msgs_LIBRARIES}
${geometric_shapes_LIBRARIES}
resource_retriever::resource_retriever
)

# 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})
ament_add_gtest(test_robot_state_benchmark test/robot_state_benchmark.cpp)
target_include_directories(test_robot_state_benchmark PUBLIC
${tf2_geometry_msgs_INCLUDE_DIRS}
${urdf_INCLUDE_DIRS}
${visualization_msgs_INCLUDE_DIRS}
${geometric_shapes_INCLUDE_DIRS}
${srdfdom_INCLUDE_DIRS}
)

target_link_libraries(test_robot_state_benchmark
moveit_test_utils
moveit_utils
moveit_exceptions
${tf2_geometry_msgs_LIBRARIES}
${srdfdom_LIBRARIES}
${MOVEIT_LIB_NAME}
${moveit_profiler_LIBRARIES}
${urdf_LIBRARIES}
${visualization_msgs_LIBRARIES}
${geometric_shapes_LIBRARIES}
resource_retriever::resource_retriever
)

ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp)
target_include_directories(test_robot_state_complex PUBLIC
${tf2_geometry_msgs_INCLUDE_DIRS}
${urdf_INCLUDE_DIRS}
${visualization_msgs_INCLUDE_DIRS}
${geometric_shapes_INCLUDE_DIRS}
${srdfdom_INCLUDE_DIRS}
)

target_link_libraries(test_robot_state_complex
moveit_test_utils
moveit_utils
moveit_exceptions
${tf2_geometry_msgs_LIBRARIES}
${srdfdom_LIBRARIES}
${MOVEIT_LIB_NAME}
${moveit_profiler_LIBRARIES}
${urdf_LIBRARIES}
${visualization_msgs_LIBRARIES}
${geometric_shapes_LIBRARIES}
resource_retriever::resource_retriever
)

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})
ament_add_gtest(test_aabb test/test_aabb.cpp)
target_include_directories(test_aabb PUBLIC
${tf2_geometry_msgs_INCLUDE_DIRS}
${urdf_INCLUDE_DIRS}
${visualization_msgs_INCLUDE_DIRS}
${geometric_shapes_INCLUDE_DIRS}
${srdfdom_INCLUDE_DIRS}
)

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
moveit_utils
moveit_exceptions
${tf2_geometry_msgs_LIBRARIES}
${srdfdom_LIBRARIES}
${MOVEIT_LIB_NAME}
${moveit_profiler_LIBRARIES}
${urdf_LIBRARIES}
${visualization_msgs_LIBRARIES}
${geometric_shapes_LIBRARIES}
resource_retriever::resource_retriever
)
endif()
8 changes: 4 additions & 4 deletions moveit_core/robot_state/include/moveit/robot_state/attached_body.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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();

Expand Down Expand Up @@ -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_;
}
Expand Down Expand Up @@ -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_;
Expand Down
6 changes: 3 additions & 3 deletions moveit_core/robot_state/include/moveit/robot_state/conversions.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand All @@ -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);

/**
Expand Down
Loading