Skip to content

Commit 5c7deaa

Browse files
davetcolemanrhaschke
davetcoleman
authored and
rhaschke
committed
Eigen::Affine3d -> Eigen::Isometry3d (#39)
1 parent afdd265 commit 5c7deaa

File tree

8 files changed

+64
-57
lines changed

8 files changed

+64
-57
lines changed

‎MIGRATION.md‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
# Migration Notes
2+
3+
API changes in MoveIt! Visual Tools releases
4+
5+
## ROS Melodic
6+
7+
- Affine3d no longer used, replaced by more computationally efficient Isometry3d. Simple find-replace should suffice

‎include/moveit_visual_tools/imarker_end_effector.h‎

Lines changed: 5 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -62,7 +62,7 @@ namespace moveit_visual_tools
6262
using visualization_msgs::InteractiveMarkerFeedback;
6363
using visualization_msgs::InteractiveMarkerControl;
6464

65-
typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Affine3d&)>
65+
typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Isometry3d&)>
6666
IMarkerCallback;
6767

6868
class IMarkerRobotState;
@@ -81,17 +81,17 @@ class IMarkerEndEffector
8181
}
8282

8383
/** \brief Get the current end effector pose */
84-
void getPose(Eigen::Affine3d& pose);
84+
void getPose(Eigen::Isometry3d& pose);
8585

8686
bool setPoseFromRobotState();
8787

8888
void iMarkerCallback(const visualization_msgs::InteractiveMarkerFeedbackConstPtr& feedback);
8989

90-
void solveIK(Eigen::Affine3d& pose);
90+
void solveIK(Eigen::Isometry3d& pose);
9191

9292
void initializeInteractiveMarkers();
9393

94-
void updateIMarkerPose(const Eigen::Affine3d& pose);
94+
void updateIMarkerPose(const Eigen::Isometry3d& pose);
9595

9696
void sendUpdatedIMarkerPose();
9797

@@ -135,7 +135,7 @@ class IMarkerEndEffector
135135

136136
// State
137137
moveit::core::RobotStatePtr imarker_state_;
138-
Eigen::Affine3d imarker_pose_;
138+
Eigen::Isometry3d imarker_pose_;
139139

140140
// Core MoveIt components
141141
planning_scene_monitor::PlanningSceneMonitorPtr psm_;

‎include/moveit_visual_tools/imarker_robot_state.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -57,7 +57,7 @@ namespace moveit_visual_tools
5757
using visualization_msgs::InteractiveMarkerFeedback;
5858
using visualization_msgs::InteractiveMarkerControl;
5959

60-
typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Affine3d&)>
60+
typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Isometry3d&)>
6161
IMarkerCallback;
6262

6363
typedef std::shared_ptr<interactive_markers::InteractiveMarkerServer> InteractiveMarkerServerPtr;
@@ -138,7 +138,7 @@ class IMarkerRobotState
138138
return name_to_eef_[name];
139139
}
140140

141-
bool setFromPoses(const EigenSTL::vector_Affine3d poses, const moveit::core::JointModelGroup* group);
141+
bool setFromPoses(const EigenSTL::vector_Isometry3d poses, const moveit::core::JointModelGroup* group);
142142

143143
protected:
144144
// --------------------------------------------------------

‎include/moveit_visual_tools/moveit_visual_tools.h‎

Lines changed: 12 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -140,7 +140,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
140140
* \param name - semantic name of MoveIt collision object
141141
* \return true on success
142142
*/
143-
bool moveCollisionObject(const Eigen::Affine3d& pose, const std::string& name,
143+
bool moveCollisionObject(const Eigen::Isometry3d& pose, const std::string& name,
144144
const rviz_visual_tools::colors& color);
145145
bool moveCollisionObject(const geometry_msgs::Pose& pose, const std::string& name,
146146
const rviz_visual_tools::colors& color);
@@ -219,14 +219,14 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
219219
* \param color to display the collision object with
220220
* \return true on success
221221
*/
222-
bool publishEEMarkers(const Eigen::Affine3d& pose, const robot_model::JointModelGroup* ee_jmg,
222+
bool publishEEMarkers(const Eigen::Isometry3d& pose, const robot_model::JointModelGroup* ee_jmg,
223223
const std::vector<double>& ee_joint_pos,
224224
const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
225225
const std::string& ns = "end_effector")
226226
{
227227
return publishEEMarkers(convertPose(pose), ee_jmg, ee_joint_pos, color, ns);
228228
}
229-
bool publishEEMarkers(const Eigen::Affine3d& pose, const robot_model::JointModelGroup* ee_jmg,
229+
bool publishEEMarkers(const Eigen::Isometry3d& pose, const robot_model::JointModelGroup* ee_jmg,
230230
const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
231231
const std::string& ns = "end_effector")
232232
{
@@ -357,7 +357,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
357357
* \param color to display the collision object with
358358
* \return true on sucess
359359
**/
360-
bool publishCollisionCuboid(const Eigen::Affine3d& pose, double width, double depth, double height,
360+
bool publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
361361
const std::string& name, const rviz_visual_tools::colors& color);
362362

363363
bool publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, double height,
@@ -387,7 +387,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
387387
* \param color to display the collision object with
388388
* \return true on sucess
389389
*/
390-
bool publishCollisionCylinder(const Eigen::Affine3d& object_pose, const std::string& object_name, double radius,
390+
bool publishCollisionCylinder(const Eigen::Isometry3d& object_pose, const std::string& object_name, double radius,
391391
double height, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
392392
bool publishCollisionCylinder(const geometry_msgs::Pose& object_pose, const std::string& object_name, double radius,
393393
double height, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
@@ -403,10 +403,10 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
403403
bool publishCollisionMesh(const geometry_msgs::Pose& object_pose, const std::string& object_name,
404404
const std::string& mesh_path,
405405
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
406-
bool publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
406+
bool publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name,
407407
const std::string& mesh_path,
408408
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
409-
bool publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
409+
bool publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name,
410410
const shape_msgs::Mesh& mesh_msg,
411411
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
412412
bool publishCollisionMesh(const geometry_msgs::Pose& object_pose, const std::string& object_name,
@@ -476,7 +476,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
476476
* \return true on success
477477
*/
478478
bool loadCollisionSceneFromFile(const std::string& path);
479-
bool loadCollisionSceneFromFile(const std::string& path, const Eigen::Affine3d& offset);
479+
bool loadCollisionSceneFromFile(const std::string& path, const Eigen::Isometry3d& offset);
480480

481481
/**
482482
* \brief Display size of workspace used for planning with OMPL, etc. Important for virtual joints
@@ -576,7 +576,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
576576
const rviz_visual_tools::colors& color = rviz_visual_tools::YELLOW);
577577

578578
/** \brief All published robot states will have their virtual joint moved by offset */
579-
void enableRobotStateRootOffet(const Eigen::Affine3d& offset);
579+
void enableRobotStateRootOffet(const Eigen::Isometry3d& offset);
580580

581581
/** \brief Turn off the root offset feature */
582582
void disableRobotStateRootOffet();
@@ -620,7 +620,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
620620
bool hideRobot();
621621

622622
/** \brief Before publishing a robot state, optionally change its root transform */
623-
static bool applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Affine3d& offset);
623+
static bool applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Isometry3d& offset);
624624

625625
/**
626626
* \brief Print to console the current robot state's joint values within its limits visually
@@ -660,7 +660,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
660660

661661
// End Effector Markers
662662
std::map<const robot_model::JointModelGroup*, visualization_msgs::MarkerArray> ee_markers_map_;
663-
std::map<const robot_model::JointModelGroup*, EigenSTL::vector_Affine3d> ee_poses_map_;
663+
std::map<const robot_model::JointModelGroup*, EigenSTL::vector_Isometry3d> ee_poses_map_;
664664
std::map<const robot_model::JointModelGroup*, std::vector<double> > ee_joint_pos_map_;
665665

666666
// Cached robot state marker - cache the colored links.
@@ -683,7 +683,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
683683

684684
// Optional offset that can be applied to all outgoing/published robot states
685685
bool robot_state_root_offset_enabled_ = false;
686-
Eigen::Affine3d robot_state_root_offset_;
686+
Eigen::Isometry3d robot_state_root_offset_;
687687

688688
public:
689689
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html

‎src/imarker_end_effector.cpp‎

Lines changed: 4 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -73,7 +73,7 @@ IMarkerEndEffector::IMarkerEndEffector(IMarkerRobotState* imarker_parent, const
7373
<< arm_data_.ee_link_->getName() << "' ready.");
7474
}
7575

76-
void IMarkerEndEffector::getPose(Eigen::Affine3d& pose)
76+
void IMarkerEndEffector::getPose(Eigen::Isometry3d& pose)
7777
{
7878
pose = imarker_pose_;
7979
}
@@ -116,7 +116,7 @@ void IMarkerEndEffector::iMarkerCallback(const visualization_msgs::InteractiveMa
116116
}
117117

118118
// Convert
119-
Eigen::Affine3d robot_ee_pose;
119+
Eigen::Isometry3d robot_ee_pose;
120120
tf::poseMsgToEigen(feedback->pose, robot_ee_pose);
121121

122122
// Update robot
@@ -133,7 +133,7 @@ void IMarkerEndEffector::iMarkerCallback(const visualization_msgs::InteractiveMa
133133
}
134134
}
135135

136-
void IMarkerEndEffector::solveIK(Eigen::Affine3d& pose)
136+
void IMarkerEndEffector::solveIK(Eigen::Isometry3d& pose)
137137
{
138138
// Cartesian settings
139139
const std::size_t attempts = 2;
@@ -177,7 +177,7 @@ void IMarkerEndEffector::initializeInteractiveMarkers()
177177
make6DofMarker(pose_msg);
178178
}
179179

180-
void IMarkerEndEffector::updateIMarkerPose(const Eigen::Affine3d& pose)
180+
void IMarkerEndEffector::updateIMarkerPose(const Eigen::Isometry3d& pose)
181181
{
182182
// Move marker to tip of fingers
183183
// imarker_pose_ = pose * imarker_offset_.inverse();

‎src/imarker_robot_state.cpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -272,7 +272,7 @@ bool IMarkerRobotState::getFilePath(std::string& file_path, const std::string& f
272272
return true;
273273
}
274274

275-
bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Affine3d poses, const moveit::core::JointModelGroup* group)
275+
bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses, const moveit::core::JointModelGroup* group)
276276
{
277277
std::vector<std::string> tips;
278278
for (std::size_t i = 0; i < arm_datas_.size(); ++i)

‎src/moveit_visual_tools.cpp‎

Lines changed: 22 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -163,7 +163,7 @@ bool MoveItVisualTools::processAttachedCollisionObjectMsg(const moveit_msgs::Att
163163
return true;
164164
}
165165

166-
bool MoveItVisualTools::moveCollisionObject(const Eigen::Affine3d& pose, const std::string& name,
166+
bool MoveItVisualTools::moveCollisionObject(const Eigen::Isometry3d& pose, const std::string& name,
167167
const rviz_visual_tools::colors& color)
168168
{
169169
return moveCollisionObject(convertPose(pose), name, color);
@@ -294,8 +294,8 @@ bool MoveItVisualTools::loadEEMarker(const robot_model::JointModelGroup* ee_jmg,
294294
// ROS_DEBUG_STREAM_NAMED(name_,"EE Parent link: " << ee_parent_link_name);
295295
const moveit::core::LinkModel* ee_parent_link = robot_model_->getLinkModel(ee_parent_link_name);
296296

297-
Eigen::Affine3d ee_marker_global_transform = shared_robot_state_->getGlobalLinkTransform(ee_parent_link);
298-
Eigen::Affine3d ee_marker_pose;
297+
Eigen::Isometry3d ee_marker_global_transform = shared_robot_state_->getGlobalLinkTransform(ee_parent_link);
298+
Eigen::Isometry3d ee_marker_pose;
299299

300300
// Process each link of the end effector
301301
for (std::size_t i = 0; i < ee_markers_map_[ee_jmg].markers.size(); ++i)
@@ -369,8 +369,8 @@ bool MoveItVisualTools::publishEEMarkers(const geometry_msgs::Pose& pose, const
369369
}
370370
}
371371

372-
Eigen::Affine3d eigen_goal_ee_pose = convertPose(pose);
373-
Eigen::Affine3d eigen_this_marker;
372+
Eigen::Isometry3d eigen_goal_ee_pose = convertPose(pose);
373+
Eigen::Isometry3d eigen_this_marker;
374374

375375
// publishArrow( pose, rviz_visual_tools::RED, rviz_visual_tools::LARGE );
376376

@@ -459,12 +459,12 @@ bool MoveItVisualTools::publishAnimatedGrasp(const moveit_msgs::Grasp& grasp,
459459
ros::Duration(0.5).sleep();
460460
}
461461

462-
Eigen::Affine3d grasp_pose_eigen;
462+
Eigen::Isometry3d grasp_pose_eigen;
463463
tf2::fromMsg(grasp_pose, grasp_pose_eigen);
464464

465465
// Pre-grasp pose variables
466466
geometry_msgs::Pose pre_grasp_pose;
467-
Eigen::Affine3d pre_grasp_pose_eigen;
467+
Eigen::Isometry3d pre_grasp_pose_eigen;
468468

469469
// Approach direction variables
470470
Eigen::Vector3d pre_grasp_approach_direction_local;
@@ -688,7 +688,7 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point
688688
return processCollisionObjectMsg(collision_obj, color);
689689
}
690690

691-
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Affine3d& pose, double width, double depth, double height,
691+
bool MoveItVisualTools::publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
692692
const std::string& name, const rviz_visual_tools::colors& color)
693693
{
694694
geometry_msgs::Pose pose_msg = tf2::toMsg(pose);
@@ -765,18 +765,18 @@ bool MoveItVisualTools::publishCollisionCylinder(const Eigen::Vector3d& a, const
765765
Eigen::Vector3d pt_center = getCenterPoint(a, b);
766766

767767
// Create vector
768-
Eigen::Affine3d pose;
768+
Eigen::Isometry3d pose;
769769
pose = getVectorBetweenPoints(pt_center, b);
770770

771771
// Convert pose to be normal to cylindar axis
772-
Eigen::Affine3d rotation;
772+
Eigen::Isometry3d rotation;
773773
rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
774774
pose = pose * rotation;
775775

776776
return publishCollisionCylinder(pose, object_name, radius, height, color);
777777
}
778778

779-
bool MoveItVisualTools::publishCollisionCylinder(const Eigen::Affine3d& object_pose, const std::string& object_name,
779+
bool MoveItVisualTools::publishCollisionCylinder(const Eigen::Isometry3d& object_pose, const std::string& object_name,
780780
double radius, double height, const rviz_visual_tools::colors& color)
781781
{
782782
return publishCollisionCylinder(convertPose(object_pose), object_name, radius, height, color);
@@ -803,7 +803,7 @@ bool MoveItVisualTools::publishCollisionCylinder(const geometry_msgs::Pose& obje
803803
return processCollisionObjectMsg(collision_obj, color);
804804
}
805805

806-
bool MoveItVisualTools::publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
806+
bool MoveItVisualTools::publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name,
807807
const std::string& mesh_path, const rviz_visual_tools::colors& color)
808808
{
809809
return publishCollisionMesh(convertPose(object_pose), object_name, mesh_path, color);
@@ -827,7 +827,7 @@ bool MoveItVisualTools::publishCollisionMesh(const geometry_msgs::Pose& object_p
827827
return true;
828828
}
829829

830-
bool MoveItVisualTools::publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
830+
bool MoveItVisualTools::publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name,
831831
const shape_msgs::Mesh& mesh_msg, const rviz_visual_tools::colors& color)
832832
{
833833
return publishCollisionMesh(convertPose(object_pose), object_name, mesh_msg, color);
@@ -894,11 +894,11 @@ bool MoveItVisualTools::publishCollisionGraph(const graph_msgs::GeometryGraph& g
894894
Eigen::Vector3d pt_center = getCenterPoint(a, b);
895895

896896
// Create vector
897-
Eigen::Affine3d pose;
897+
Eigen::Isometry3d pose;
898898
pose = getVectorBetweenPoints(pt_center, b);
899899

900900
// Convert pose to be normal to cylindar axis
901-
Eigen::Affine3d rotation;
901+
Eigen::Isometry3d rotation;
902902
rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
903903
pose = pose * rotation;
904904

@@ -1017,10 +1017,10 @@ bool MoveItVisualTools::publishCollisionTable(double x, double y, double z, doub
10171017

10181018
bool MoveItVisualTools::loadCollisionSceneFromFile(const std::string& path)
10191019
{
1020-
return loadCollisionSceneFromFile(path, Eigen::Affine3d::Identity());
1020+
return loadCollisionSceneFromFile(path, Eigen::Isometry3d::Identity());
10211021
}
10221022

1023-
bool MoveItVisualTools::loadCollisionSceneFromFile(const std::string& path, const Eigen::Affine3d& offset)
1023+
bool MoveItVisualTools::loadCollisionSceneFromFile(const std::string& path, const Eigen::Isometry3d& offset)
10241024
{
10251025
// Open file
10261026
std::ifstream fin(path.c_str());
@@ -1277,7 +1277,7 @@ bool MoveItVisualTools::publishTrajectoryLine(const robot_trajectory::RobotTraje
12771277
// Visualize end effector position of cartesian path
12781278
for (std::size_t i = 0; i < robot_trajectory.getWayPointCount(); ++i)
12791279
{
1280-
const Eigen::Affine3d& tip_pose = robot_trajectory.getWayPoint(i).getGlobalLinkTransform(ee_parent_link);
1280+
const Eigen::Isometry3d& tip_pose = robot_trajectory.getWayPoint(i).getGlobalLinkTransform(ee_parent_link);
12811281

12821282
// Error Check
12831283
if (tip_pose.translation().x() != tip_pose.translation().x())
@@ -1352,14 +1352,14 @@ bool MoveItVisualTools::publishTrajectoryPoints(const std::vector<robot_state::R
13521352
// Visualize end effector position of cartesian path
13531353
for (std::size_t i = 0; i < robot_state_trajectory.size(); ++i)
13541354
{
1355-
const Eigen::Affine3d& tip_pose = robot_state_trajectory[i]->getGlobalLinkTransform(ee_parent_link);
1355+
const Eigen::Isometry3d& tip_pose = robot_state_trajectory[i]->getGlobalLinkTransform(ee_parent_link);
13561356

13571357
publishSphere(tip_pose, color);
13581358
}
13591359
return true;
13601360
}
13611361

1362-
void MoveItVisualTools::enableRobotStateRootOffet(const Eigen::Affine3d& offset)
1362+
void MoveItVisualTools::enableRobotStateRootOffet(const Eigen::Isometry3d& offset)
13631363
{
13641364
robot_state_root_offset_enabled_ = true;
13651365
robot_state_root_offset_ = offset;
@@ -1462,7 +1462,7 @@ bool MoveItVisualTools::hideRobot()
14621462
loadSharedRobotState();
14631463

14641464
// Apply transform
1465-
Eigen::Affine3d offset;
1465+
Eigen::Isometry3d offset;
14661466
offset.translation().x() = rviz_visual_tools::LARGE_SCALE;
14671467
offset.translation().y() = rviz_visual_tools::LARGE_SCALE;
14681468
offset.translation().z() = rviz_visual_tools::LARGE_SCALE;
@@ -1573,7 +1573,7 @@ bool MoveItVisualTools::checkForVirtualJoint(const moveit::core::RobotState& rob
15731573
return true;
15741574
}
15751575

1576-
bool MoveItVisualTools::applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Affine3d& offset)
1576+
bool MoveItVisualTools::applyVirtualJointTransform(moveit::core::RobotState& robot_state, const Eigen::Isometry3d& offset)
15771577
{
15781578
static const std::string VJOINT_NAME = "virtual_joint";
15791579

0 commit comments

Comments
 (0)