From d66b2262edd3f05429c6e49068c547bdac903ebb Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 28 Aug 2022 11:13:23 +0200 Subject: [PATCH] Rename variables in visualizePlan() - link_pose -> start_pose - pos_link -> pos_start --- core/src/stages/move_relative.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 0489445b..30af9aa5 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -104,8 +104,9 @@ static bool getJointStateFromOffset(const boost::any& direction, const moveit::c return false; } +// Create an arrow marker from start_pose to reached_pose, split into a red and green part based on achieved distance 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 std::string& ns, const std::string& frame_id, const Eigen::Isometry3d& start_pose, const Eigen::Isometry3d& reached_pose, const Eigen::Vector3d& linear, double distance) { double linear_norm = linear.norm(); @@ -114,7 +115,7 @@ static void visualizePlan(std::deque& markers, Inter 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_start = start_pose.translation(); Eigen::Vector3d pos_reached = reached_pose.translation(); Eigen::Vector3d pos_target = pos_reached + quat_target * Eigen::Vector3d(linear_norm - distance, 0, 0); @@ -124,7 +125,7 @@ static void visualizePlan(std::deque& markers, Inter 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::makeArrow(m, pos_start, pos_reached, 0.1 * linear_norm); rviz_marker_tools::setColor(m.color, rviz_marker_tools::LIME_GREEN); markers.push_back(m); } else { @@ -139,14 +140,14 @@ static void visualizePlan(std::deque& markers, Inter 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 - m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_link + pos_reached) }); + m.pose.position = tf2::toMsg(Eigen::Vector3d{ 0.5 * (pos_start + pos_reached) }); m.pose.orientation = tf2::toMsg(quat_cylinder); 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::makeArrow(m, pos_reached, pos_start, 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) {