mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add visualization for Point goals (#264)
- move visualization from `getPoseGoal` to `compute` - create target frame from `target_eigen` to allow visualization from Pose and Point goals
This commit is contained in:
parent
ba0b4f6146
commit
d71c645d83
@ -97,12 +97,10 @@ protected:
|
|||||||
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& trajectory,
|
bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene, SubTrajectory& trajectory,
|
||||||
Interface::Direction dir) override;
|
Interface::Direction dir) override;
|
||||||
bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state);
|
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,
|
bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
|
||||||
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
|
Eigen::Isometry3d& target_eigen);
|
||||||
decltype(std::declval<SolutionBase>().markers())& markers);
|
|
||||||
bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
|
bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
|
||||||
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
|
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen);
|
||||||
decltype(std::declval<SolutionBase>().markers())& /*unused*/);
|
|
||||||
|
|
||||||
protected:
|
protected:
|
||||||
solvers::PlannerInterfacePtr planner_;
|
solvers::PlannerInterfacePtr planner_;
|
||||||
|
|||||||
@ -141,9 +141,8 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
|
|||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
|
|
||||||
bool MoveTo::getPoseGoal(const boost::any& goal, const geometry_msgs::PoseStamped& ik_pose_msg,
|
bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
|
||||||
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
|
Eigen::Isometry3d& target_eigen) {
|
||||||
decltype(std::declval<SolutionBase>().markers())& markers) {
|
|
||||||
try {
|
try {
|
||||||
const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal);
|
const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal);
|
||||||
tf::poseMsgToEigen(target.pose, target_eigen);
|
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
|
// transform target into global frame
|
||||||
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
|
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
|
||||||
target_eigen = frame * target_eigen;
|
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&) {
|
} catch (const boost::bad_any_cast&) {
|
||||||
return false;
|
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,
|
bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
|
||||||
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen,
|
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) {
|
||||||
decltype(std::declval<SolutionBase>().markers())& /*unused*/) {
|
|
||||||
try {
|
try {
|
||||||
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
|
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
|
||||||
Eigen::Vector3d target_point;
|
Eigen::Vector3d target_point;
|
||||||
@ -178,8 +170,6 @@ bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel*
|
|||||||
// retain link orientation
|
// retain link orientation
|
||||||
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link);
|
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link);
|
||||||
target_eigen.translation() = target_point;
|
target_eigen.translation() = target_point;
|
||||||
|
|
||||||
// TODO: add marker visualization
|
|
||||||
} catch (const boost::bad_any_cast&) {
|
} catch (const boost::bad_any_cast&) {
|
||||||
return false;
|
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()) &&
|
if (!getPoseGoal(goal, scene, target_eigen) && !getPointGoal(goal, link, scene, target_eigen)) {
|
||||||
!getPointGoal(goal, link, scene, target_eigen, solution.markers())) {
|
|
||||||
solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
|
solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
|
||||||
return false;
|
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
|
// transform target pose such that ik frame will reach there if link does
|
||||||
Eigen::Isometry3d ik_pose;
|
Eigen::Isometry3d ik_pose;
|
||||||
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
|
tf::poseMsgToEigen(ik_pose_msg.pose, ik_pose);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user