diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index c22979b8..e4d98125 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -104,6 +104,63 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c return false; } +static void visualizePlan(std::deque& 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("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("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); } } diff --git a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h index 3944d24a..f4e3e55d 100644 --- a/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h +++ b/rviz_marker_tools/include/rviz_marker_tools/marker_creation.h @@ -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); diff --git a/rviz_marker_tools/src/marker_creation.cpp b/rviz_marker_tools/src/marker_creation.cpp index 12ad7797..47a5c2f9 100644 --- a/rviz_marker_tools/src/marker_creation.cpp +++ b/rviz_marker_tools/src/marker_creation.cpp @@ -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;