Skip to content

Commit

Permalink
Eigen::Affine3d -> Eigen::Isometry3d (#39)
Browse files Browse the repository at this point in the history
  • Loading branch information
@davetcoleman @rhaschke
davetcoleman authored and rhaschke committed Nov 28, 2018
1 parent afdd265 commit 5c7deaa
Show file tree
Hide file tree
Showing 8 changed files with 64 additions and 57 deletions.
7 changes: 7 additions & 0 deletions MIGRATION.md
View file
@@ -0,0 +1,7 @@
# Migration Notes

API changes in MoveIt! Visual Tools releases

## ROS Melodic

- Affine3d no longer used, replaced by more computationally efficient Isometry3d. Simple find-replace should suffice
10 changes: 5 additions & 5 deletions include/moveit_visual_tools/imarker_end_effector.h
View file
Expand Up @@ -62,7 +62,7 @@ namespace moveit_visual_tools
using visualization_msgs::InteractiveMarkerFeedback;
using visualization_msgs::InteractiveMarkerControl;

typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Affine3d&)>
typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Isometry3d&)>
IMarkerCallback;

class IMarkerRobotState;
Expand All @@ -81,17 +81,17 @@ class IMarkerEndEffector
}

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

bool setPoseFromRobotState();

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

void solveIK(Eigen::Affine3d& pose);
void solveIK(Eigen::Isometry3d& pose);

void initializeInteractiveMarkers();

void updateIMarkerPose(const Eigen::Affine3d& pose);
void updateIMarkerPose(const Eigen::Isometry3d& pose);

void sendUpdatedIMarkerPose();

Expand Down Expand Up @@ -135,7 +135,7 @@ class IMarkerEndEffector

// State
moveit::core::RobotStatePtr imarker_state_;
Eigen::Affine3d imarker_pose_;
Eigen::Isometry3d imarker_pose_;

// Core MoveIt components
planning_scene_monitor::PlanningSceneMonitorPtr psm_;
Expand Down
4 changes: 2 additions & 2 deletions include/moveit_visual_tools/imarker_robot_state.h
View file
Expand Up @@ -57,7 +57,7 @@ namespace moveit_visual_tools
using visualization_msgs::InteractiveMarkerFeedback;
using visualization_msgs::InteractiveMarkerControl;

typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Affine3d&)>
typedef std::function<void(const visualization_msgs::InteractiveMarkerFeedbackConstPtr&, const Eigen::Isometry3d&)>
IMarkerCallback;

typedef std::shared_ptr<interactive_markers::InteractiveMarkerServer> InteractiveMarkerServerPtr;
Expand Down Expand Up @@ -138,7 +138,7 @@ class IMarkerRobotState
return name_to_eef_[name];
}

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

protected:
// --------------------------------------------------------
Expand Down
24 changes: 12 additions & 12 deletions include/moveit_visual_tools/moveit_visual_tools.h
View file
Expand Up @@ -140,7 +140,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
* \param name - semantic name of MoveIt collision object
* \return true on success
*/
bool moveCollisionObject(const Eigen::Affine3d& pose, const std::string& name,
bool moveCollisionObject(const Eigen::Isometry3d& pose, const std::string& name,
const rviz_visual_tools::colors& color);
bool moveCollisionObject(const geometry_msgs::Pose& pose, const std::string& name,
const rviz_visual_tools::colors& color);
Expand Down Expand Up @@ -219,14 +219,14 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
* \param color to display the collision object with
* \return true on success
*/
bool publishEEMarkers(const Eigen::Affine3d& pose, const robot_model::JointModelGroup* ee_jmg,
bool publishEEMarkers(const Eigen::Isometry3d& pose, const robot_model::JointModelGroup* ee_jmg,
const std::vector<double>& ee_joint_pos,
const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
const std::string& ns = "end_effector")
{
return publishEEMarkers(convertPose(pose), ee_jmg, ee_joint_pos, color, ns);
}
bool publishEEMarkers(const Eigen::Affine3d& pose, const robot_model::JointModelGroup* ee_jmg,
bool publishEEMarkers(const Eigen::Isometry3d& pose, const robot_model::JointModelGroup* ee_jmg,
const rviz_visual_tools::colors& color = rviz_visual_tools::CLEAR,
const std::string& ns = "end_effector")
{
Expand Down Expand Up @@ -357,7 +357,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
* \param color to display the collision object with
* \return true on sucess
**/
bool publishCollisionCuboid(const Eigen::Affine3d& pose, double width, double depth, double height,
bool publishCollisionCuboid(const Eigen::Isometry3d& pose, double width, double depth, double height,
const std::string& name, const rviz_visual_tools::colors& color);

bool publishCollisionCuboid(const geometry_msgs::Pose& pose, double width, double depth, double height,
Expand Down Expand Up @@ -387,7 +387,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
* \param color to display the collision object with
* \return true on sucess
*/
bool publishCollisionCylinder(const Eigen::Affine3d& object_pose, const std::string& object_name, double radius,
bool publishCollisionCylinder(const Eigen::Isometry3d& object_pose, const std::string& object_name, double radius,
double height, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
bool publishCollisionCylinder(const geometry_msgs::Pose& object_pose, const std::string& object_name, double radius,
double height, const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
Expand All @@ -403,10 +403,10 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
bool publishCollisionMesh(const geometry_msgs::Pose& object_pose, const std::string& object_name,
const std::string& mesh_path,
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
bool publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
bool publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name,
const std::string& mesh_path,
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
bool publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
bool publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name,
const shape_msgs::Mesh& mesh_msg,
const rviz_visual_tools::colors& color = rviz_visual_tools::GREEN);
bool publishCollisionMesh(const geometry_msgs::Pose& object_pose, const std::string& object_name,
Expand Down Expand Up @@ -476,7 +476,7 @@ class MoveItVisualTools : public rviz_visual_tools::RvizVisualTools
* \return true on success
*/
bool loadCollisionSceneFromFile(const std::string& path);
bool loadCollisionSceneFromFile(const std::string& path, const Eigen::Affine3d& offset);
bool loadCollisionSceneFromFile(const std::string& path, const Eigen::Isometry3d& offset);

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

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

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

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

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

// End Effector Markers
std::map<const robot_model::JointModelGroup*, visualization_msgs::MarkerArray> ee_markers_map_;
std::map<const robot_model::JointModelGroup*, EigenSTL::vector_Affine3d> ee_poses_map_;
std::map<const robot_model::JointModelGroup*, EigenSTL::vector_Isometry3d> ee_poses_map_;
std::map<const robot_model::JointModelGroup*, std::vector<double> > ee_joint_pos_map_;

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

// Optional offset that can be applied to all outgoing/published robot states
bool robot_state_root_offset_enabled_ = false;
Eigen::Affine3d robot_state_root_offset_;
Eigen::Isometry3d robot_state_root_offset_;

public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW // http://eigen.tuxfamily.org/dox/group__TopicStructHavingEigenMembers.html
Expand Down
8 changes: 4 additions & 4 deletions src/imarker_end_effector.cpp
View file
Expand Up @@ -73,7 +73,7 @@ IMarkerEndEffector::IMarkerEndEffector(IMarkerRobotState* imarker_parent, const
<< arm_data_.ee_link_->getName() << "' ready.");
}

void IMarkerEndEffector::getPose(Eigen::Affine3d& pose)
void IMarkerEndEffector::getPose(Eigen::Isometry3d& pose)
{
pose = imarker_pose_;
}
Expand Down Expand Up @@ -116,7 +116,7 @@ void IMarkerEndEffector::iMarkerCallback(const visualization_msgs::InteractiveMa
}

// Convert
Eigen::Affine3d robot_ee_pose;
Eigen::Isometry3d robot_ee_pose;
tf::poseMsgToEigen(feedback->pose, robot_ee_pose);

// Update robot
Expand All @@ -133,7 +133,7 @@ void IMarkerEndEffector::iMarkerCallback(const visualization_msgs::InteractiveMa
}
}

void IMarkerEndEffector::solveIK(Eigen::Affine3d& pose)
void IMarkerEndEffector::solveIK(Eigen::Isometry3d& pose)
{
// Cartesian settings
const std::size_t attempts = 2;
Expand Down Expand Up @@ -177,7 +177,7 @@ void IMarkerEndEffector::initializeInteractiveMarkers()
make6DofMarker(pose_msg);
}

void IMarkerEndEffector::updateIMarkerPose(const Eigen::Affine3d& pose)
void IMarkerEndEffector::updateIMarkerPose(const Eigen::Isometry3d& pose)
{
// Move marker to tip of fingers
// imarker_pose_ = pose * imarker_offset_.inverse();
Expand Down
2 changes: 1 addition & 1 deletion src/imarker_robot_state.cpp
View file
Expand Up @@ -272,7 +272,7 @@ bool IMarkerRobotState::getFilePath(std::string& file_path, const std::string& f
return true;
}

bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Affine3d poses, const moveit::core::JointModelGroup* group)
bool IMarkerRobotState::setFromPoses(const EigenSTL::vector_Isometry3d poses, const moveit::core::JointModelGroup* group)
{
std::vector<std::string> tips;
for (std::size_t i = 0; i < arm_datas_.size(); ++i)
Expand Down
44 changes: 22 additions & 22 deletions src/moveit_visual_tools.cpp
View file
Expand Up @@ -163,7 +163,7 @@ bool MoveItVisualTools::processAttachedCollisionObjectMsg(const moveit_msgs::Att
return true;
}

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

Eigen::Affine3d ee_marker_global_transform = shared_robot_state_->getGlobalLinkTransform(ee_parent_link);
Eigen::Affine3d ee_marker_pose;
Eigen::Isometry3d ee_marker_global_transform = shared_robot_state_->getGlobalLinkTransform(ee_parent_link);
Eigen::Isometry3d ee_marker_pose;

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

Eigen::Affine3d eigen_goal_ee_pose = convertPose(pose);
Eigen::Affine3d eigen_this_marker;
Eigen::Isometry3d eigen_goal_ee_pose = convertPose(pose);
Eigen::Isometry3d eigen_this_marker;

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

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

Eigen::Affine3d grasp_pose_eigen;
Eigen::Isometry3d grasp_pose_eigen;
tf2::fromMsg(grasp_pose, grasp_pose_eigen);

// Pre-grasp pose variables
geometry_msgs::Pose pre_grasp_pose;
Eigen::Affine3d pre_grasp_pose_eigen;
Eigen::Isometry3d pre_grasp_pose_eigen;

// Approach direction variables
Eigen::Vector3d pre_grasp_approach_direction_local;
Expand Down Expand Up @@ -688,7 +688,7 @@ bool MoveItVisualTools::publishCollisionCuboid(const geometry_msgs::Point& point
return processCollisionObjectMsg(collision_obj, color);
}

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

// Create vector
Eigen::Affine3d pose;
Eigen::Isometry3d pose;
pose = getVectorBetweenPoints(pt_center, b);

// Convert pose to be normal to cylindar axis
Eigen::Affine3d rotation;
Eigen::Isometry3d rotation;
rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
pose = pose * rotation;

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

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

bool MoveItVisualTools::publishCollisionMesh(const Eigen::Affine3d& object_pose, const std::string& object_name,
bool MoveItVisualTools::publishCollisionMesh(const Eigen::Isometry3d& object_pose, const std::string& object_name,
const std::string& mesh_path, const rviz_visual_tools::colors& color)
{
return publishCollisionMesh(convertPose(object_pose), object_name, mesh_path, color);
Expand All @@ -827,7 +827,7 @@ bool MoveItVisualTools::publishCollisionMesh(const geometry_msgs::Pose& object_p
return true;
}

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

// Create vector
Eigen::Affine3d pose;
Eigen::Isometry3d pose;
pose = getVectorBetweenPoints(pt_center, b);

// Convert pose to be normal to cylindar axis
Eigen::Affine3d rotation;
Eigen::Isometry3d rotation;
rotation = Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
pose = pose * rotation;

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

bool MoveItVisualTools::loadCollisionSceneFromFile(const std::string& path)
{
return loadCollisionSceneFromFile(path, Eigen::Affine3d::Identity());
return loadCollisionSceneFromFile(path, Eigen::Isometry3d::Identity());
}

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

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

publishSphere(tip_pose, color);
}
return true;
}

void MoveItVisualTools::enableRobotStateRootOffet(const Eigen::Affine3d& offset)
void MoveItVisualTools::enableRobotStateRootOffet(const Eigen::Isometry3d& offset)
{
robot_state_root_offset_enabled_ = true;
robot_state_root_offset_ = offset;
Expand Down Expand Up @@ -1462,7 +1462,7 @@ bool MoveItVisualTools::hideRobot()
loadSharedRobotState();

// Apply transform
Eigen::Affine3d offset;
Eigen::Isometry3d offset;
offset.translation().x() = rviz_visual_tools::LARGE_SCALE;
offset.translation().y() = rviz_visual_tools::LARGE_SCALE;
offset.translation().z() = rviz_visual_tools::LARGE_SCALE;
Expand Down Expand Up @@ -1573,7 +1573,7 @@ bool MoveItVisualTools::checkForVirtualJoint(const moveit::core::RobotState& rob
return true;
}

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

Expand Down

0 comments on commit 5c7deaa

Please sign in to comment.