mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Rename variables in visualizePlan()
- link_pose -> start_pose - pos_link -> pos_start
This commit is contained in:
parent
32d3454c1f
commit
d66b2262ed
@ -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<visualization_msgs::Marker>& 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<visualization_msgs::Marker>& 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<visualization_msgs::Marker>& 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<visualization_msgs::Marker>& 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) {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user