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:
j-kuehn 2021-05-03 21:29:05 +02:00 committed by GitHub
parent 93d95e394f
commit ba0b4f6146
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 93 additions and 32 deletions

View File

@ -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);
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) {
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);
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();
}
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);
}
}

View File

@ -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);

View File

@ -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;