diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 180ee4c8..98bf0f57 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -97,12 +97,10 @@ protected: bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& trajectory, Interface::Direction dir) override; bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state); - bool getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg, - const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen, - decltype(std::declval().markers())& markers); + bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, + Eigen::Isometry3d& target_eigen); bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link, - const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen, - decltype(std::declval().markers())& /*unused*/); + const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen); protected: solvers::PlannerInterfacePtr planner_; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index c613aee9..d505fbc4 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -141,9 +141,8 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint return false; } -bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg, - const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen, - decltype(std::declval().markers())& markers) { +bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, + Eigen::Isometry3d& target_eigen) { try { const geometry_msgs::PoseStamped& target = boost::any_cast(goal); tf::poseMsgToEigen(target.pose, target_eigen); @@ -151,12 +150,6 @@ bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStampe // transform target into global frame const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id); target_eigen = frame * target_eigen; - - // frame at target pose - rviz_marker_tools::appendFrame(markers, target, 0.1, "ik frame"); - - // frame at link - rviz_marker_tools::appendFrame(markers, ik_pose_msg, 0.1, "ik frame"); } catch (const boost::bad_any_cast&) { return false; } @@ -164,8 +157,7 @@ bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStampe } bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link, - const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen, - decltype(std::declval().markers())& /*unused*/) { + const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) { try { const geometry_msgs::PointStamped& target = boost::any_cast(goal); Eigen::Vector3d target_point; @@ -178,8 +170,6 @@ bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* // retain link orientation target_eigen = scene->getCurrentState().getGlobalLinkTransform(link); target_eigen.translation() = target_point; - - // TODO: add marker visualization } catch (const boost::bad_any_cast&) { return false; } @@ -236,12 +226,18 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP } } - if (!getPoseGoal(goal, ik_pose_msg, scene, target_eigen, solution.markers()) && - !getPointGoal(goal, link, scene, target_eigen, solution.markers())) { + if (!getPoseGoal(goal, scene, target_eigen) && !getPointGoal(goal, link, scene, target_eigen)) { solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name()); return false; } + // visualize plan with frame at target pose and frame at link + geometry_msgs::PoseStamped target; + target.header.frame_id = scene->getPlanningFrame(); + tf::poseEigenToMsg(target_eigen, target.pose); + rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); + // transform target pose such that ik frame will reach there if link does Eigen::Isometry3d ik_pose; tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);