Skip to content
Permalink
Browse files

MoveIt! tf2 migration (#830)

migration from tf to tf2 API, resolves issue #745

- All type conversions now depend on geometry2 ROS packages, rather than geometry
  (see ros/geometry2#292 and
  ros/geometry2#294 for details of the new conversions)
- Removes all boost::shared_ptr<tf::TransformListener> from the API,
  and replaced them with std::shared_ptr<tf2_ros::Buffer>'s
- Utilize new tf2 API in the tf::Transformer library to access the internal tf2::Buffer of RViz
  (see ros/geometry#163 for details of the new API)
- Removes prepending of forward slashes ('/') for transforms frames as this is deprecated in tf2
- Replaced deprecated tf2 _getLatestCommonTime
  • Loading branch information...
IanTheEngineer authored and rhaschke committed May 18, 2018
1 parent 6cca255 commit 736251dc77a24877135522eb901ae40a1ea9872b
Showing with 586 additions and 587 deletions.
  1. +3 −2 moveit_core/CMakeLists.txt
  2. +4 −4 moveit_core/collision_detection/src/collision_tools.cpp
  3. +2 −3 moveit_core/constraint_samplers/CMakeLists.txt
  4. +0 −1 moveit_core/constraint_samplers/src/default_constraint_samplers.cpp
  5. +2 −5 moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp
  6. +0 −1 moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.h
  7. +5 −5 moveit_core/distance_field/src/distance_field.cpp
  8. +2 −2 moveit_core/distance_field/test/test_distance_field.cpp
  9. +6 −6 moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
  10. +0 −1 moveit_core/kinematic_constraints/src/utils.cpp
  11. +2 −4 moveit_core/kinematic_constraints/test/test_constraints.cpp
  12. +4 −3 moveit_core/package.xml
  13. +13 −15 moveit_core/planning_scene/src/planning_scene.cpp
  14. +1 −3 moveit_core/robot_model/src/robot_model.cpp
  15. +10 −11 moveit_core/robot_state/src/conversions.cpp
  16. +9 −9 moveit_core/robot_state/src/robot_state.cpp
  17. +4 −5 moveit_core/robot_trajectory/src/robot_trajectory.cpp
  18. +7 −30 moveit_core/transforms/src/transforms.cpp
  19. +1 −2 moveit_experimental/CMakeLists.txt
  20. +3 −3 moveit_experimental/collision_distance_field/src/collision_common_distance_field.cpp
  21. +2 −2 moveit_experimental/collision_distance_field/src/collision_robot_distance_field.cpp
  22. +4 −4 moveit_experimental/kinematics_constraint_aware/src/kinematics_constraint_aware.cpp
  23. +2 −3 moveit_experimental/package.xml
  24. +2 −2 moveit_kinematics/CMakeLists.txt
  25. +2 −2 moveit_kinematics/ikfast_kinematics_plugin/scripts/create_ikfast_moveit_plugin.py
  26. +2 −2 moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt
  27. +5 −5 moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp
  28. +5 −7 moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp
  29. +3 −6 moveit_kinematics/lma_kinematics_plugin/src/lma_kinematics_plugin.cpp
  30. +3 −0 moveit_kinematics/package.xml
  31. +0 −4 moveit_planners/chomp/chomp_interface/include/chomp_interface/chomp_planning_context.h
  32. +0 −2 moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp
  33. +0 −1 moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp
  34. +1 −2 moveit_planners/ompl/CMakeLists.txt
  35. +0 −1 moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp
  36. +7 −3 moveit_planners/ompl/ompl_interface/src/ompl_planner.cpp
  37. +2 −4 moveit_planners/ompl/package.xml
  38. +2 −0 moveit_ros/benchmarks/CMakeLists.txt
  39. +5 −6 moveit_ros/benchmarks/benchmarks/src/benchmark_execution.cpp
  40. +2 −0 moveit_ros/benchmarks/package.xml
  41. +3 −3 moveit_ros/benchmarks/src/BenchmarkExecutor.cpp
  42. +1 −1 moveit_ros/manipulation/CMakeLists.txt
  43. +0 −2 moveit_ros/manipulation/move_group_pick_place_capability/src/pick_place_action_capability.cpp
  44. +2 −2 moveit_ros/manipulation/package.xml
  45. +0 −1 moveit_ros/manipulation/pick_place/include/moveit/pick_place/manipulation_pipeline.h
  46. +3 −3 moveit_ros/manipulation/pick_place/src/approach_and_translate_stage.cpp
  47. +0 −1 moveit_ros/manipulation/pick_place/src/pick_place.cpp
  48. +3 −3 moveit_ros/manipulation/pick_place/src/place.cpp
  49. +3 −3 moveit_ros/manipulation/pick_place/src/reachable_valid_pose_filter.cpp
  50. +3 −1 moveit_ros/move_group/CMakeLists.txt
  51. +6 −2 moveit_ros/move_group/package.xml
  52. +3 −3 moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp
  53. +1 −0 moveit_ros/move_group/src/default_capabilities/execute_trajectory_service_capability.h
  54. +3 −3 moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp
  55. +0 −1 moveit_ros/move_group/src/default_capabilities/state_validation_service_capability.cpp
  56. +5 −3 moveit_ros/move_group/src/move_group.cpp
  57. +7 −13 moveit_ros/move_group/src/move_group_capability.cpp
  58. +3 −2 moveit_ros/perception/CMakeLists.txt
  59. +2 −2 ...th_image_octomap_updater/include/moveit/depth_image_octomap_updater/depth_image_octomap_updater.h
  60. +14 −10 moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp
  61. +4 −2 moveit_ros/perception/mesh_filter/include/moveit/mesh_filter/transform_provider.h
  62. +25 −16 moveit_ros/perception/mesh_filter/src/transform_provider.cpp
  63. +6 −6 ...ros/perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_monitor.h
  64. +0 −1 ...ros/perception/occupancy_map_monitor/include/moveit/occupancy_map_monitor/occupancy_map_updater.h
  65. +17 −7 moveit_ros/perception/occupancy_map_monitor/src/occupancy_map_monitor.cpp
  66. +5 −5 moveit_ros/perception/occupancy_map_monitor/src/occupancy_map_server.cpp
  67. +8 −4 moveit_ros/perception/package.xml
  68. +6 −4 ...pointcloud_octomap_updater/include/moveit/pointcloud_octomap_updater/pointcloud_octomap_updater.h
  69. +16 −12 moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp
  70. +5 −5 moveit_ros/perception/semantic_world/src/semantic_world.cpp
  71. +5 −3 moveit_ros/planning/CMakeLists.txt
  72. +10 −2 moveit_ros/planning/package.xml
  73. +7 −3 moveit_ros/planning/planning_components_tools/src/visualize_robot_collision_volume.cpp
  74. +9 −9 ...ros/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/current_state_monitor.h
  75. +16 −15 ...os/planning/planning_scene_monitor/include/moveit/planning_scene_monitor/planning_scene_monitor.h
  76. +22 −28 moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp
  77. +40 −67 moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp
  78. +2 −2 moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp
  79. +5 −3 moveit_ros/planning_interface/CMakeLists.txt
  80. +6 −6 ...mmon_planning_interface_objects/include/moveit/common_planning_interface_objects/common_objects.h
  81. +16 −11 moveit_ros/planning_interface/common_planning_interface_objects/src/common_objects.cpp
  82. +10 −8 ...lanning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h
  83. +37 −43 moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp
  84. +17 −10 moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp
  85. +10 −6 moveit_ros/planning_interface/package.xml
  86. +4 −3 moveit_ros/robot_interaction/CMakeLists.txt
  87. +0 −1 moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction.h
  88. +6 −6 moveit_ros/robot_interaction/include/moveit/robot_interaction/interaction_handler.h
  89. +8 −4 moveit_ros/robot_interaction/package.xml
  90. +20 −20 moveit_ros/robot_interaction/src/interaction_handler.cpp
  91. +11 −8 moveit_ros/robot_interaction/src/interactive_marker_helpers.cpp
  92. +21 −20 moveit_ros/robot_interaction/src/robot_interaction.cpp
  93. +1 −2 moveit_ros/visualization/CMakeLists.txt
  94. +3 −1 moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp
  95. +2 −2 moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_objects.cpp
  96. +0 −2 moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame_scenes.cpp
  97. +2 −0 moveit_ros/visualization/package.xml
  98. +2 −2 moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp
  99. +2 −1 moveit_ros/warehouse/CMakeLists.txt
  100. +4 −2 moveit_ros/warehouse/package.xml
  101. +2 −2 moveit_ros/warehouse/warehouse/src/import_from_text.cpp
  102. +5 −2 moveit_ros/warehouse/warehouse/src/save_to_warehouse.cpp
  103. +0 −1 moveit_setup_assistant/include/moveit/setup_assistant/tools/moveit_config_data.h
@@ -32,7 +32,8 @@ find_package(urdfdom_headers REQUIRED)

find_package(catkin REQUIRED
COMPONENTS
eigen_conversions
tf2_eigen
tf2_kdl
eigen_stl_containers
geometric_shapes
geometry_msgs
@@ -111,7 +112,7 @@ catkin_package(
moveit_dynamics_solver
${OCTOMAP_LIBRARIES}
CATKIN_DEPENDS
eigen_conversions
tf2_eigen
eigen_stl_containers
geometric_shapes
geometry_msgs
@@ -35,7 +35,7 @@
/* Author: Ioan Sucan */

#include <moveit/collision_detection/collision_tools.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>

namespace collision_detection
{
@@ -272,8 +272,8 @@ void costSourceToMsg(const CostSource& cost_source, moveit_msgs::CostSource& msg

void contactToMsg(const Contact& contact, moveit_msgs::ContactInformation& msg)
{
tf::pointEigenToMsg(contact.pos, msg.position);
tf::vectorEigenToMsg(contact.normal, msg.normal);
msg.position = tf2::toMsg(contact.pos);
tf2::toMsg(contact.normal, msg.normal);
msg.depth = contact.depth;
msg.contact_body_1 = contact.body_name_1;
msg.contact_body_2 = contact.body_name_2;
@@ -291,4 +291,4 @@ void contactToMsg(const Contact& contact, moveit_msgs::ContactInformation& msg)
msg.body_type_2 = moveit_msgs::ContactInformation::WORLD_OBJECT;
}

} // end of namespace collision_detection
} // end of namespace collision_detection
@@ -26,10 +26,10 @@ install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION})
if(CATKIN_ENABLE_TESTING)
find_package(orocos_kdl REQUIRED)
find_package(angles REQUIRED)
find_package(tf_conversions REQUIRED)
find_package(tf2_kdl REQUIRED)
find_package(moveit_resources REQUIRED)

include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf_conversions_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS})
include_directories(${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS} ${moveit_resources_INCLUDE_DIRS})

catkin_add_gtest(test_constraint_samplers
test/test_constraint_samplers.cpp
@@ -41,7 +41,6 @@ if(CATKIN_ENABLE_TESTING)
${catkin_LIBRARIES}
${angles_LIBRARIES}
${orocos_kdl_LIBRARIES}
${tf_conversions_LIBRARIES}
${urdfdom_LIBRARIES}
${urdfdom_headers_LIBRARIES}
)
@@ -37,7 +37,6 @@
#include <moveit/constraint_samplers/default_constraint_samplers.h>
#include <set>
#include <cassert>
#include <eigen_conversions/eigen_msg.h>
#include <boost/bind.hpp>

namespace constraint_samplers
@@ -36,8 +36,7 @@

#include <geometry_msgs/PoseStamped.h>
#include <kdl_parser/kdl_parser.hpp>
#include <eigen_conversions/eigen_msg.h>
#include <eigen_conversions/eigen_kdl.h>
#include <tf2_kdl/tf2_kdl.h>
#include <algorithm>
#include <numeric>

@@ -340,9 +339,7 @@ bool PR2ArmKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose
return false;
}
KDL::Frame pose_desired;
Eigen::Affine3d tp;
tf::poseMsgToEigen(ik_pose, tp);
tf::transformEigenToKDL(tp, pose_desired);
tf2::fromMsg(ik_pose, pose_desired);

// Do the IK
KDL::JntArray jnt_pos_in;
@@ -43,7 +43,6 @@
#include <kdl_parser/kdl_parser.hpp>

#include <angles/angles.h>
#include <tf_conversions/tf_kdl.h>

#include <moveit/macros/class_forward.h>
#include <moveit_msgs/GetPositionFK.h>
@@ -37,7 +37,7 @@
#include <moveit/distance_field/distance_field.h>
#include <moveit/distance_field/find_internal_points.h>
#include <geometric_shapes/body_operations.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <ros/console.h>
#include <octomap/octomap.h>
#include <octomap/OcTree.h>
@@ -229,7 +229,7 @@ void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Aff
void DistanceField::addShapeToField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
{
Eigen::Affine3d pose_e;
tf::poseMsgToEigen(pose, pose_e);
tf2::fromMsg(pose, pose_e);
addShapeToField(shape, pose_e);
}

@@ -310,8 +310,8 @@ void DistanceField::moveShapeInField(const shapes::Shape* shape, const geometry_
const geometry_msgs::Pose& new_pose)
{
Eigen::Affine3d old_pose_e, new_pose_e;
tf::poseMsgToEigen(old_pose, old_pose_e);
tf::poseMsgToEigen(new_pose, new_pose_e);
tf2::fromMsg(old_pose, old_pose_e);
tf2::fromMsg(new_pose, new_pose_e);
moveShapeInField(shape, old_pose_e, new_pose_e);
}

@@ -329,7 +329,7 @@ void DistanceField::removeShapeFromField(const shapes::Shape* shape, const Eigen
void DistanceField::removeShapeFromField(const shapes::Shape* shape, const geometry_msgs::Pose& pose)
{
Eigen::Affine3d pose_e;
tf::poseMsgToEigen(pose, pose_e);
tf2::fromMsg(pose, pose_e);
removeShapeFromField(shape, pose_e);
}

@@ -40,7 +40,7 @@
#include <moveit/distance_field/propagation_distance_field.h>
#include <moveit/distance_field/find_internal_points.h>
#include <geometric_shapes/body_operations.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <octomap/octomap.h>
#include <ros/console.h>

@@ -511,7 +511,7 @@ TEST(TestSignedPropagationDistanceField, TestSignedAddRemovePoints)
p.position.z = .5;

Eigen::Affine3d p_eigen;
tf::poseMsgToEigen(p, p_eigen);
tf2::fromMsg(p, p_eigen);

gradient_df.addShapeToField(&sphere, p_eigen);
// printBoth(gradient_df, numX, numY, numZ);
@@ -41,7 +41,7 @@
#include <moveit/collision_detection_fcl/collision_robot_fcl.h>
#include <moveit/collision_detection_fcl/collision_world_fcl.h>
#include <boost/math/constants/constants.hpp>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/bind.hpp>
#include <limits>
#include <memory>
@@ -319,7 +319,7 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co
}
constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
Eigen::Affine3d t;
tf::poseMsgToEigen(pc.constraint_region.primitive_poses[i], t);
tf2::fromMsg(pc.constraint_region.primitive_poses[i], t);
constraint_region_pose_.push_back(t);
if (mobile_frame_)
constraint_region_.back()->setPose(constraint_region_pose_.back());
@@ -346,7 +346,7 @@ bool PositionConstraint::configure(const moveit_msgs::PositionConstraint& pc, co
}
constraint_region_.push_back(bodies::BodyPtr(bodies::createBodyFromShape(shape.get())));
Eigen::Affine3d t;
tf::poseMsgToEigen(pc.constraint_region.mesh_poses[i], t);
tf2::fromMsg(pc.constraint_region.mesh_poses[i], t);
constraint_region_pose_.push_back(t);
if (mobile_frame_)
constraint_region_.back()->setPose(constraint_region_pose_.back());
@@ -504,7 +504,7 @@ bool OrientationConstraint::configure(const moveit_msgs::OrientationConstraint&
return false;
}
Eigen::Quaterniond q;
tf::quaternionMsgToEigen(oc.orientation, q);
tf2::fromMsg(oc.orientation, q);
if (fabs(q.norm() - 1.0) > 1e-3)
{
ROS_WARN_NAMED("kinematic_constraints", "Orientation constraint for link '%s' is probably incorrect: %f, %f, %f, "
@@ -698,7 +698,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc
points_.push_back(Eigen::Vector3d(x, y, 0.0));
}

tf::poseMsgToEigen(vc.target_pose.pose, target_pose_);
tf2::fromMsg(vc.target_pose.pose, target_pose_);

if (tf.isFixedFrame(vc.target_pose.header.frame_id))
{
@@ -715,7 +715,7 @@ bool VisibilityConstraint::configure(const moveit_msgs::VisibilityConstraint& vc
mobile_target_frame_ = true;
}

tf::poseMsgToEigen(vc.sensor_pose.pose, sensor_pose_);
tf2::fromMsg(vc.sensor_pose.pose, sensor_pose_);

if (tf.isFixedFrame(vc.sensor_pose.header.frame_id))
{
@@ -36,7 +36,6 @@

#include <moveit/kinematic_constraints/utils.h>
#include <geometric_shapes/solid_primitive_dims.h>
#include <eigen_conversions/eigen_msg.h>

moveit_msgs::Constraints kinematic_constraints::mergeConstraints(const moveit_msgs::Constraints& first,
const moveit_msgs::Constraints& second)
@@ -38,7 +38,7 @@
#include <gtest/gtest.h>
#include <urdf_parser/urdf_parser.h>
#include <fstream>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <boost/filesystem/path.hpp>
#include <moveit_resources/config.h>

@@ -659,9 +659,7 @@ TEST_F(LoadPlanningModelsPr2, OrientationConstraintsSimple)

ASSERT_TRUE(oc.getLinkModel());

geometry_msgs::Pose p;

tf::poseEigenToMsg(ks.getGlobalLinkTransform(oc.getLinkModel()->getName()), p);
geometry_msgs::Pose p = tf2::toMsg(ks.getGlobalLinkTransform(oc.getLinkModel()->getName()));

ocm.orientation = p.orientation;
ocm.header.frame_id = kmodel->getModelFrame();
@@ -22,7 +22,6 @@
<build_depend>assimp</build_depend>
<build_depend>boost</build_depend>
<build_depend>eigen</build_depend>
<build_depend>eigen_conversions</build_depend>
<build_depend>eigen_stl_containers</build_depend>
<build_depend>libfcl-dev</build_depend>
<build_depend version_gte="0.5.2">geometric_shapes</build_depend>
@@ -43,13 +42,14 @@
<build_depend>shape_msgs</build_depend>
<build_depend>srdfdom</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>tf2_eigen</build_depend>
<build_depend>tf2_kdl</build_depend>
<build_depend>trajectory_msgs</build_depend>
<build_depend>visualization_msgs</build_depend>

<run_depend>assimp</run_depend>
<run_depend>boost</run_depend>
<run_depend>eigen</run_depend>
<run_depend>eigen_conversions</run_depend>
<run_depend>eigen_stl_containers</run_depend>
<run_depend>libfcl-dev</run_depend>
<run_depend version_gte="0.5.2">geometric_shapes</run_depend>
@@ -68,12 +68,13 @@
<run_depend>sensor_msgs</run_depend>
<run_depend>srdfdom</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>tf2_eigen</run_depend>
<run_depend>tf2_kdl</run_depend>
<run_depend>trajectory_msgs</run_depend>
<run_depend>visualization_msgs</run_depend>

<test_depend>angles</test_depend>
<test_depend>moveit_resources</test_depend>
<test_depend>orocos_kdl</test_depend>
<test_depend>rosunit</test_depend>
<test_depend>tf_conversions</test_depend>
</package>
@@ -44,7 +44,7 @@
#include <moveit/exceptions/exceptions.h>
#include <moveit/robot_state/attached_body.h>
#include <octomap_msgs/conversions.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_eigen/tf2_eigen.h>
#include <memory>
#include <set>

@@ -821,9 +821,7 @@ bool PlanningScene::getCollisionObjectMsg(moveit_msgs::CollisionObject& collisio
shapes::ShapeMsg sm;
if (constructMsgFromShape(obj->shapes_[j].get(), sm))
{
geometry_msgs::Pose p;
tf::poseEigenToMsg(obj->shape_poses_[j], p);

geometry_msgs::Pose p = tf2::toMsg(obj->shape_poses_[j]);
sv.setPoseMessage(&p);
boost::apply_visitor(sv, sm);
}
@@ -885,7 +883,7 @@ bool PlanningScene::getOctomapMsg(octomap_msgs::OctomapWithPose& octomap) const
{
const shapes::OcTree* o = static_cast<const shapes::OcTree*>(map->shapes_[0].get());
octomap_msgs::fullMapToMsg(*o->octree, octomap.octomap);
tf::poseEigenToMsg(map->shape_poses_[0], octomap.origin);
octomap.origin = tf2::toMsg(map->shape_poses_[0]);
return true;
}
ROS_ERROR_NAMED("planning_scene",
@@ -1359,7 +1357,7 @@ void PlanningScene::processOctomapMsg(const octomap_msgs::OctomapWithPose& map)
std::shared_ptr<octomap::OcTree> om(static_cast<octomap::OcTree*>(octomap_msgs::msgToMap(map.octomap)));
const Eigen::Affine3d& t = getTransforms().getTransform(map.header.frame_id);
Eigen::Affine3d p;
tf::poseMsgToEigen(map.origin, p);
tf2::fromMsg(map.origin, p);
p = t * p;
world_->addToObject(OCTOMAP_NS, shapes::ShapeConstPtr(new shapes::OcTree(om)), p);
}
@@ -1500,7 +1498,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.object.primitive_poses[i], p);
tf2::fromMsg(object.object.primitive_poses[i], p);
shapes.push_back(shapes::ShapeConstPtr(s));
poses.push_back(p);
}
@@ -1511,7 +1509,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.object.mesh_poses[i], p);
tf2::fromMsg(object.object.mesh_poses[i], p);
shapes.push_back(shapes::ShapeConstPtr(s));
poses.push_back(p);
}
@@ -1522,7 +1520,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::Attache
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.object.plane_poses[i], p);
tf2::fromMsg(object.object.plane_poses[i], p);
shapes.push_back(shapes::ShapeConstPtr(s));
poses.push_back(p);
}
@@ -1698,7 +1696,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.primitive_poses[i], p);
tf2::fromMsg(object.primitive_poses[i], p);
world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
}
}
@@ -1708,7 +1706,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.mesh_poses[i], p);
tf2::fromMsg(object.mesh_poses[i], p);
world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
}
}
@@ -1718,7 +1716,7 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
if (s)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.plane_poses[i], p);
tf2::fromMsg(object.plane_poses[i], p);
world_->addToObject(object.id, shapes::ShapeConstPtr(s), t * p);
}
}
@@ -1754,19 +1752,19 @@ bool PlanningScene::processCollisionObjectMsg(const moveit_msgs::CollisionObject
for (std::size_t i = 0; i < object.primitive_poses.size(); ++i)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.primitive_poses[i], p);
tf2::fromMsg(object.primitive_poses[i], p);
new_poses.push_back(t * p);
}
for (std::size_t i = 0; i < object.mesh_poses.size(); ++i)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.mesh_poses[i], p);
tf2::fromMsg(object.mesh_poses[i], p);
new_poses.push_back(t * p);
}
for (std::size_t i = 0; i < object.plane_poses.size(); ++i)
{
Eigen::Affine3d p;
tf::poseMsgToEigen(object.plane_poses[i], p);
tf2::fromMsg(object.plane_poses[i], p);
new_poses.push_back(t * p);
}

0 comments on commit 736251d

Please sign in to comment.
You can’t perform that action at this time.