mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Improve arrow visualization for MoveRelative stage (#255)
Implement visualization as red-green arrow * overload makeArrow to allow creation with points * create new function for visualization * if no plan is found, construct arrow from green cylinder and red arrow * adjust arrow construction for backward propagators
This commit is contained in:
parent
93d95e394f
commit
ba0b4f6146
@ -104,6 +104,63 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c
|
||||
return false;
|
||||
}
|
||||
|
||||
static void visualizePlan(std::deque<visualization_msgs::Marker>& markers, Interface::Direction dir, bool success,
|
||||
const std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& link_pose,
|
||||
const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) {
|
||||
double linear_norm = linear.norm();
|
||||
|
||||
// rotation of the target direction and for the cylinder marker
|
||||
auto quat_target = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear);
|
||||
auto quat_cylinder = quat_target * Eigen::AngleAxisd(0.5 * M_PI, Eigen::Vector3d::UnitY());
|
||||
|
||||
// link position before planning; reached link position after planning; target link position
|
||||
Eigen::Vector3d pos_link = link_pose.translation();
|
||||
Eigen::Vector3d pos_reached = reached_pose.translation();
|
||||
Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0);
|
||||
|
||||
visualization_msgs::Marker m;
|
||||
m.ns = ns;
|
||||
m.header.frame_id = frame_id;
|
||||
if (dir == Interface::FORWARD) {
|
||||
if (success) {
|
||||
// valid part: green arrow
|
||||
rviz_marker_tools::makeArrow(m, pos_link, pos_reached, 0.1 * linear_norm);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
markers.push_back(m);
|
||||
} else {
|
||||
// invalid part: red arrow
|
||||
// set head length to keep default shaft:head proportion of 1:0.3 as defined in
|
||||
// rviz/default_plugin/markers/arrow_marker.cpp#L105
|
||||
rviz_marker_tools::makeArrow(m, pos_reached, pos_target, 0.1 * linear_norm, 0.23 * linear_norm);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::RED);
|
||||
markers.push_back(m);
|
||||
|
||||
// valid part: green cylinder
|
||||
rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, distance);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
// position half-way between pos_link and pos_reached
|
||||
tf::pointEigenToMsg(0.5 * (pos_link + pos_reached), m.pose.position);
|
||||
tf::quaternionEigenToMsg(quat_cylinder, m.pose.orientation);
|
||||
markers.push_back(m);
|
||||
}
|
||||
} else {
|
||||
// valid part: green arrow
|
||||
// head length according to above comment
|
||||
rviz_marker_tools::makeArrow(m, pos_reached, pos_link, 0.1 * linear_norm, 0.23 * linear_norm);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN);
|
||||
markers.push_back(m);
|
||||
if (!success) {
|
||||
// invalid part: red cylinder
|
||||
rviz_marker_tools::makeCylinder(m, 0.1 * linear_norm, linear_norm - distance);
|
||||
rviz_marker_tools::setColor(m.color, rviz_marker_tools::RED);
|
||||
// position half-way between pos_reached and pos_target
|
||||
tf::pointEigenToMsg(0.5 * (pos_reached + pos_target), m.pose.position);
|
||||
tf::quaternionEigenToMsg(quat_cylinder, m.pose.orientation);
|
||||
markers.push_back(m);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool MoveRelative::compute(const InterfaceState& state, planning_scene::PlanningScenePtr& scene,
|
||||
SubTrajectory& solution, Interface::Direction dir) {
|
||||
scene = state.scene()->diff();
|
||||
@ -240,19 +297,21 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
|
||||
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||
|
||||
// min_distance reached?
|
||||
if (min_distance > 0.0) {
|
||||
double distance = 0.0;
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||
robot_state::RobotStatePtr& reached_state = robot_trajectory->getLastWayPointPtr();
|
||||
reached_state->updateLinkTransforms();
|
||||
const Eigen::Isometry3d& reached_pose = reached_state->getGlobalLinkTransform(link);
|
||||
|
||||
double distance = 0.0;
|
||||
if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) {
|
||||
if (use_rotation_distance) {
|
||||
Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose());
|
||||
distance = rotation.angle();
|
||||
} else
|
||||
distance = (reached_pose.translation() - link_pose.translation()).norm();
|
||||
}
|
||||
|
||||
// min_distance reached?
|
||||
if (min_distance > 0.0) {
|
||||
success = distance >= min_distance;
|
||||
if (!success) {
|
||||
char msg[100];
|
||||
@ -264,27 +323,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
||||
} else if (!success)
|
||||
solution.setComment("failed to move full distance");
|
||||
|
||||
// add an arrow marker
|
||||
visualization_msgs::Marker m;
|
||||
m.ns = props.get<std::string>("marker_ns");
|
||||
if (!m.ns.empty()) {
|
||||
m.header.frame_id = scene->getPlanningFrame();
|
||||
if (linear_norm > 1e-3) {
|
||||
// +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder)
|
||||
rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN : rviz_marker_tools::RED);
|
||||
rviz_marker_tools::makeArrow(m, linear_norm);
|
||||
auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear);
|
||||
Eigen::Vector3d pos(link_pose.translation());
|
||||
if (dir == Interface::BACKWARD) {
|
||||
// flip arrow direction
|
||||
quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY());
|
||||
// arrow tip at goal pose
|
||||
pos += quat * Eigen::Vector3d(-linear_norm, 0, 0);
|
||||
}
|
||||
tf::pointEigenToMsg(pos, m.pose.position);
|
||||
tf::quaternionEigenToMsg(quat, m.pose.orientation);
|
||||
solution.markers().push_back(m);
|
||||
}
|
||||
// visualize plan
|
||||
auto ns = props.get<std::string>("marker_ns");
|
||||
if (!ns.empty() && linear_norm > 0) { // ensures that 'distance' is the norm of the reached distance
|
||||
visualizePlan(solution.markers(), dir, success, ns, scene->getPlanningFrame(), link_pose, reached_pose, linear,
|
||||
distance);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -65,6 +65,10 @@ inline visualization_msgs::Marker& makeMesh(visualization_msgs::Marker& m, const
|
||||
return makeMesh(m, filename, scale, scale, scale);
|
||||
}
|
||||
|
||||
/// create an arrow with a start and end point
|
||||
visualization_msgs::Marker& makeArrow(visualization_msgs::Marker& m, const Eigen::Vector3d& start_point,
|
||||
const Eigen::Vector3d& end_point, double diameter, double head_length = 0.0);
|
||||
|
||||
/// create an arrow along x-axis
|
||||
visualization_msgs::Marker& makeArrow(visualization_msgs::Marker& m, double scale = 1.0, bool tip_at_origin = false);
|
||||
|
||||
|
||||
@ -276,6 +276,20 @@ vm::Marker& makeMesh(vm::Marker& m, const std::string& filename, double sx, doub
|
||||
return m;
|
||||
}
|
||||
|
||||
vm::Marker& makeArrow(vm::Marker& m, const Eigen::Vector3d& start_point, const Eigen::Vector3d& end_point,
|
||||
double diameter, double head_length) {
|
||||
// scale.y is set according to default proportions in rviz/default_plugin/markers/arrow_marker.cpp#L61
|
||||
// for the default head_length=0, the head length will keep the default proportion defined in arrow_marker.cpp#L106
|
||||
m.scale.x = diameter;
|
||||
m.scale.y = 2 * diameter;
|
||||
m.scale.z = head_length;
|
||||
prepareMarker(m, vm::Marker::ARROW);
|
||||
m.points.resize(2);
|
||||
tf::pointEigenToMsg(start_point, m.points[0]);
|
||||
tf::pointEigenToMsg(end_point, m.points[1]);
|
||||
return m;
|
||||
}
|
||||
|
||||
vm::Marker& makeArrow(vm::Marker& m, double scale, bool tip_at_origin) {
|
||||
m.scale.y = m.scale.z = 0.1 * scale;
|
||||
m.scale.x = scale;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user