diff --git a/source/armarx/navigation/components/navigator/Component.cpp b/source/armarx/navigation/components/navigator/Component.cpp
index 34d27ae7372d410a97cdab175d67543b0f4ad074..3509d2f331fd0408d1cbe57d47c6a6aaa094f778 100644
--- a/source/armarx/navigation/components/navigator/Component.cpp
+++ b/source/armarx/navigation/components/navigator/Component.cpp
@@ -258,7 +258,7 @@ namespace armarx::navigation::components::navigator
fac::NavigationStackFactory::create(stackConfig, sceneProvider->scene());
// set visualization of LocalPlanner
- stack.localPlanner->setVisualization(&getArvizClient());
+ stack.localPlanner->setVisualization(getArvizClient());
memoryIntrospectors.emplace_back(
std::make_unique<server::MemoryIntrospector>(resultsWriterPlugin->get(), callerId));
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.cpp b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
index e64958400d3982c2cc7c3efde91408faf1f6f0cf..8a6aac53e45052b0359a4a9a13ba98df51aa15ba 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.cpp
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.cpp
@@ -15,14 +15,15 @@ namespace armarx::navigation::human
.weight = params.intimateWeight};
// personal zone
- auto& global_T_human = human.pose;
- auto& global_V_offset = global_T_human * params.offset;
+ auto& global_R_human = human.pose.linear();
+ auto& global_V_offset = global_R_human * params.offset;
core::Pose2D pose = human.pose;
pose.translation() += global_V_offset;
float velocityNorm = human.linearVelocity.norm();
- float movementStretch = 1 + velocityNorm * params.movementInfluence;
+ // stretch with velocity in m/s as factor
+ float movementStretch = 1 + velocityNorm * params.movementInfluence / 1000; // [mm] to [m]
if (velocityNorm != 0)
{
// y pointing forward
diff --git a/source/armarx/navigation/human/ProxemicZoneCreator.h b/source/armarx/navigation/human/ProxemicZoneCreator.h
index d1696db4de9837d77132faf9c58583334450547b..fed4c18cce4105e0ffba8a6023bcb68268ae4b41 100644
--- a/source/armarx/navigation/human/ProxemicZoneCreator.h
+++ b/source/armarx/navigation/human/ProxemicZoneCreator.h
@@ -38,7 +38,9 @@ namespace armarx::navigation::human
float personalRadius = 1000;
float movementInfluence = 1;
- // in the coordinate system of the human
+ // an offset applied to the personal zone in the coordinate system of the human
+ // a positive x-value means an offset to the right
+ // a positive y-value means an offset to the front
Eigen::Vector2f offset{100, 150};
};
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.cpp b/source/armarx/navigation/local_planning/LocalPlanner.cpp
index 4b7ea7ed74a41cec3e7f01c31c320d77ade4a813..ab0fbd229acf07ea26833e9a77481fe923f6237e 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.cpp
+++ b/source/armarx/navigation/local_planning/LocalPlanner.cpp
@@ -7,8 +7,8 @@ namespace armarx::navigation::local_planning
}
void
- LocalPlanner::setVisualization(viz::Client* vis)
+ LocalPlanner::setVisualization(viz::Client& vis)
{
- arviz = vis;
+ arviz.emplace(vis);
}
} // namespace armarx::navigation::local_planning
diff --git a/source/armarx/navigation/local_planning/LocalPlanner.h b/source/armarx/navigation/local_planning/LocalPlanner.h
index f2f6641b622a78101a4fd5892d0170ce764c4661..031318818e623d942d7acd3469d5bf771722890f 100644
--- a/source/armarx/navigation/local_planning/LocalPlanner.h
+++ b/source/armarx/navigation/local_planning/LocalPlanner.h
@@ -58,10 +58,10 @@ namespace armarx::navigation::local_planning
virtual std::optional<LocalPlannerResult> plan(const core::GlobalTrajectory& goal) = 0;
- void setVisualization(viz::Client* vis);
+ void setVisualization(viz::Client& vis);
protected:
- viz::Client* arviz;
+ std::optional<viz::ScopedClient> arviz;
private:
const core::Scene& context;
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.cpp b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
index 5e46a90175f9455ac1a8ab42176f862605d991d4..35d3afb47d5f5b609bf138f0beecd72bfea8e4e4 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.cpp
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.cpp
@@ -29,7 +29,7 @@ namespace armarx::navigation::local_planning
auto obst = boost::make_shared<teb_local_planner::PolygonObstacle>();
const Eigen::Vector2d min = conv::toRos(bbox.getMin());
- const Eigen::Vector2d max = conv::toRos(bbox.getMin());
+ const Eigen::Vector2d max = conv::toRos(bbox.getMax());
obst->pushBackVertex(min);
obst->pushBackVertex(min.x(), max.y());
@@ -39,6 +39,7 @@ namespace armarx::navigation::local_planning
obst->finalizePolygon();
container.push_back(obst);
+ // visualize bounding box if layer is available
if (visLayer != nullptr)
{
const Eigen::Vector3f min3d = conv::fromRos(min);
@@ -58,6 +59,7 @@ namespace armarx::navigation::local_planning
{
auto proxemicZones = proxemics.createProxemicZones(human);
+ int i = 0;
for (const auto& proxemicZone : proxemicZones)
{
auto pose = conv::toRos(proxemicZone.pose);
@@ -82,16 +84,18 @@ namespace armarx::navigation::local_planning
container.push_back(obst);
+ // visualize proxemic zone if layer is available
if (visLayer != nullptr)
{
const Eigen::Vector3f axisLength(proxemicZone.shape.a, proxemicZone.shape.b, 0);
- const core::Pose pose3d = conv::to3D(human.pose);
+ const core::Pose pose3d = conv::to3D(proxemicZone.pose);
visLayer->add(viz::Ellipsoid("proxemicZone_" + std::to_string(visualizationIndex++))
.pose(pose3d)
.axisLengths(axisLength)
- .color(simox::Color::blue()));
+ .color(PROXEMIC_ZONE_COLOR[i % PROXEMIC_ZONE_COLOR.size()]));
}
+ i++;
}
}
diff --git a/source/armarx/navigation/local_planning/TebObstacleManager.h b/source/armarx/navigation/local_planning/TebObstacleManager.h
index b336b65bf32f42994851bf67edd5abab3bd752aa..b8859c4bbcb8145c3e1bb159e3ba7490331b14e4 100644
--- a/source/armarx/navigation/local_planning/TebObstacleManager.h
+++ b/source/armarx/navigation/local_planning/TebObstacleManager.h
@@ -50,6 +50,9 @@ namespace armarx::navigation::local_planning
teb_local_planner::ObstContainer& container;
human::ProxemicZoneCreator proxemics;
int visualizationIndex;
+
+ const std::array<simox::Color, 2> PROXEMIC_ZONE_COLOR = {simox::Color::red(),
+ simox::Color::blue()};
};
diff --git a/source/armarx/navigation/local_planning/TimedElasticBands.cpp b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
index 2633583dafd120cf02f10839d8cc26b131b90aae..91483e74e63a8f260f2893968ab25f9d2942658a 100644
--- a/source/armarx/navigation/local_planning/TimedElasticBands.cpp
+++ b/source/armarx/navigation/local_planning/TimedElasticBands.cpp
@@ -112,7 +112,7 @@ namespace armarx::navigation::local_planning
geometry_msgs::Twist velocity_start = conv::toRos(scene.platformVelocity);
- //TODO (SALt): fill obstacles
+
fillObstacles();
try
@@ -130,9 +130,9 @@ namespace armarx::navigation::local_planning
core::LocalTrajectory best = conv::fromRos(hcp_->findBestTeb()->teb());
// visualize path alternatives
- if (arviz != nullptr)
+ if (arviz)
{
- auto layer = arviz->layer("local_planner_path_alternatives");
+ auto layer = arviz.value().layer("local_planner_path_alternatives");
int i = 0;
int bestTebIdx = hcp_->bestTebIdx();
@@ -152,7 +152,8 @@ namespace armarx::navigation::local_planning
.color(simox::Color::gray()));
i++;
}
- arviz->commit(layer);
+
+ arviz.value().commit(layer);
}
return {{.trajectory = best}};
@@ -165,9 +166,9 @@ namespace armarx::navigation::local_planning
viz::Layer* visPtr = nullptr;
viz::Layer visLayer;
- if (arviz != nullptr)
+ if (arviz)
{
- visLayer = arviz->layer("local_planner_obstacles");
+ visLayer = arviz.value().layer("local_planner_obstacles");
visPtr = &visLayer;
}
@@ -189,9 +190,9 @@ namespace armarx::navigation::local_planning
}
}
- if (arviz != nullptr)
+ if (arviz)
{
- arviz->commit(visLayer);
+ arviz.value().commit(visLayer);
}
ARMARX_INFO << "TEB: added " << obstManager.size() << " obstacles";
diff --git a/source/armarx/navigation/local_planning/ros_conversions.cpp b/source/armarx/navigation/local_planning/ros_conversions.cpp
index 4bb21ef4249d50d7965c5fd1134844968cc1f491..86fd6a2c954ab6a9d44a44e7255eea850b2d1711 100644
--- a/source/armarx/navigation/local_planning/ros_conversions.cpp
+++ b/source/armarx/navigation/local_planning/ros_conversions.cpp
@@ -49,6 +49,8 @@ namespace armarx::navigation::conv
// we change it such that x is pointing forward.
// ROS: x pointing forward, ArmarX: y pointing forward
theta += M_PI_2;
+ // normalize angle
+ theta = g2o::normalize_theta(theta);
return {pos / 1000., theta}; // [mm] to [m]
}