diff --git a/CMakeLists.txt b/CMakeLists.txt
index 308bddb12bbc1dd0968f0348b19024e6c6bf35c0..fc6e0627c607299fc0eaa0b6a8f6a1103cc28dd8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -39,6 +39,8 @@ armarx_find_package(PUBLIC VTK QUIET)
armarx_find_package(PUBLIC SemanticObjectRelations QUIET)
armarx_find_package(PUBLIC OpenCV QUIET) # Required as RobotAPI is a legacy project.
armarx_find_package(PUBLIC range-v3 QUIET)
+armarx_find_package(PUBLIC HRVO QUIET)
+armarx_find_package(PUBLIC RVO QUIET)
# human aware navigation
armarx_find_package(PUBLIC teb_local_planner QUIET)
diff --git a/scenarios/HumanAwareNavigation/config/example_client.cfg b/scenarios/HumanAwareNavigation/config/example_client.cfg
index c21733bd95c226d102ba54430e01dc8b8b848766..ea5576f65c6fdb7ec47b104bf675aa5e3558ce75 100644
--- a/scenarios/HumanAwareNavigation/config/example_client.cfg
+++ b/scenarios/HumanAwareNavigation/config/example_client.cfg
@@ -194,6 +194,22 @@ ArmarX.ExampleClient.nav.NavigatorName = navigator
# ArmarX.example_client.ObjectName = ""
+# ArmarX.example_client.mem.nav.costmap.CoreSegment:
+# Attributes:
+# - Default: Costmap
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.mem.nav.costmap.CoreSegment = Costmap
+
+
+# ArmarX.example_client.mem.nav.costmap.Memory:
+# Attributes:
+# - Default: Navigation
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.example_client.mem.nav.costmap.Memory = Navigation
+
+
# ArmarX.example_client.mem.robot_state.Memory:
# Attributes:
# - Default: RobotState
@@ -233,7 +249,7 @@ ArmarX.ExampleClient.nav.NavigatorName = navigator
# - Default: standard
# - Case sensitivity: yes
# - Required: no
-# - Possible values: {complex, point_to_point, standard, update_navigator}
+# - Possible values: {complex, point_to_point, standard, update_navigator, wander_around}
# ArmarX.example_client.mode = standard
diff --git a/scenarios/HumanAwareNavigation/config/human_simulator.cfg b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
index c0a571fa5e9de5a86f9fecf99428a7334f2302f8..d7720e7bf82f9ffc176e204cfefd69ebf243f3f3 100644
--- a/scenarios/HumanAwareNavigation/config/human_simulator.cfg
+++ b/scenarios/HumanAwareNavigation/config/human_simulator.cfg
@@ -157,7 +157,23 @@
# - Case sensitivity: yes
# - Required: no
# - Possible values: {Debug, Error, Fatal, Important, Info, Undefined, Verbose, Warning}
-# ArmarX.Verbosity = Info
+ArmarX.Verbosity = Verbose
+
+
+# ArmarX.human_simulator.ArVizStorageName: Name of the ArViz storage
+# Attributes:
+# - Default: ArVizStorage
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.ArVizStorageName = ArVizStorage
+
+
+# ArmarX.human_simulator.ArVizTopicName: Name of the ArViz topic
+# Attributes:
+# - Default: ArVizTopic
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.ArVizTopicName = ArVizTopic
# ArmarX.human_simulator.EnableProfiling: enable profiler which is used for logging performance events
@@ -178,6 +194,14 @@
# ArmarX.human_simulator.MinimumLoggingLevel = Undefined
+# ArmarX.human_simulator.ObjectMemoryName: Name of the object memory.
+# Attributes:
+# - Default: ObjectMemory
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.ObjectMemoryName = ObjectMemory
+
+
# ArmarX.human_simulator.ObjectName: Name of IceGrid well-known object
# Attributes:
# - Default: ""
@@ -260,12 +284,28 @@
# ArmarX.human_simulator.mns.MemoryNameSystemName = MemoryNameSystem
+# ArmarX.human_simulator.p.human.goalDistanceThreshold:
+# Attributes:
+# - Default: 100
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.p.human.goalDistanceThreshold = 100
+
+
+# ArmarX.human_simulator.p.human.maxLinearVelocity:
+# Attributes:
+# - Default: 200
+# - Case sensitivity: yes
+# - Required: no
+# ArmarX.human_simulator.p.human.maxLinearVelocity = 200
+
+
# ArmarX.human_simulator.p.nHumans: Number of humans to spawn.
# Attributes:
# - Default: 4
# - Case sensitivity: yes
# - Required: no
-# ArmarX.human_simulator.p.nHumans = 4
+ArmarX.human_simulator.p.nHumans = 6
# ArmarX.human_simulator.p.taskPeriodMs:
diff --git a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
index a3e73f4f38e5be8e1545891322ab2ac4f66b7471..45901d4483977812ccf754fd73b2d6ce7fd8a3c7 100644
--- a/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
+++ b/scenarios/HumanAwareNavigation/config/navigation_memory.cfg
@@ -255,10 +255,10 @@ ArmarX.navigation_memory.MinimumLoggingLevel = Verbose
# ArmarX.navigation_memory.p.locationGraph.visuFrequency: Visualization frequeny of locations and graph edges [Hz].
# Attributes:
-# - Default: 2
+# - Default: 10
# - Case sensitivity: yes
# - Required: no
-# ArmarX.navigation_memory.p.locationGraph.visuFrequency = 2
+# ArmarX.navigation_memory.p.locationGraph.visuFrequency = 10
# ArmarX.navigation_memory.p.locationGraph.visuGraphEdges: Enable visualization of navigation graph edges.
diff --git a/source/armarx/navigation/components/human_simulator/CMakeLists.txt b/source/armarx/navigation/components/human_simulator/CMakeLists.txt
index 3ea4392a9fd911109b7f85046edec9d1c4d9469c..c5cb14c6099a66e52447e5654af7aadbf138dc7d 100644
--- a/source/armarx/navigation/components/human_simulator/CMakeLists.txt
+++ b/source/armarx/navigation/components/human_simulator/CMakeLists.txt
@@ -23,4 +23,7 @@ armarx_add_component(human_simulator
armarx_navigation::dynamic_scene
armarx_navigation::teb_human
armarx_navigation::simulation
+
+ # HRVO::HRVO
+ RVO::RVO
)
diff --git a/source/armarx/navigation/components/human_simulator/Component.cpp b/source/armarx/navigation/components/human_simulator/Component.cpp
index 3410013c006ce7d0ce85ed3a21629ff5f7430713..6ce73503ea0d6f7951fb70454ca19a89e6072bb8 100644
--- a/source/armarx/navigation/components/human_simulator/Component.cpp
+++ b/source/armarx/navigation/components/human_simulator/Component.cpp
@@ -23,17 +23,28 @@
#include "Component.h"
+#include <cstddef>
#include <cstdlib>
+#include <SimoxUtility/color/Color.h>
+#include <VirtualRobot/CollisionDetection/CollisionModel.h>
#include <VirtualRobot/Random.h>
+#include <VirtualRobot/SceneObjectSet.h>
#include "ArmarXCore/core/logging/Logging.h"
#include <ArmarXCore/libraries/DecoupledSingleComponent/Decoupled.h>
+#include "RobotAPI/components/ArViz/Client/Elements.h"
+#include "RobotAPI/components/ArViz/Client/elements/Path.h"
+#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
+
#include "armarx/navigation/algorithms/Costmap.h"
#include "armarx/navigation/core/basic_types.h"
#include "armarx/navigation/human/types.h"
#include "armarx/navigation/simulation/SimulatedHuman.h"
+#include "armarx/navigation/util/util.h"
+#include <RVOSimulator.h>
+#include <Vector2.h>
// Include headers you only need in function definitions in the .cpp.
@@ -77,8 +88,9 @@ namespace armarx::navigation::components::human_simulator
// Add an optionalproperty.
def->optional(properties.taskPeriodMs, "p.taskPeriodMs", "");
def->optional(properties.nHumans, "p.nHumans", "Number of humans to spawn.");
-
- def->optional(properties.humanParams.goalDistanceThreshold, "p.human.goalDistanceThreshold", "");
+
+ def->optional(
+ properties.humanParams.goalDistanceThreshold, "p.human.goalDistanceThreshold", "");
def->optional(properties.humanParams.minLinearVelocity, "p.human.maxLinearVelocity", "");
def->optional(properties.humanParams.maxLinearVelocity, "p.human.maxLinearVelocity", "");
@@ -96,40 +108,59 @@ namespace armarx::navigation::components::human_simulator
// setDebugObserverBatchModeEnabled(true);
}
-
void
- Component::onConnectComponent()
+ Component::setPreferredVelocities(RVO::RVOSimulator* simulator,
+ const std::vector<RVO::Vector2>& goals)
{
- // Do things after connecting to topics and components.
+ /* Set the preferred velocity to be a vector of unit magnitude (speed) in the
+ * direction of the goal. */
+ // #ifdef _OPENMP
+ // #pragma omp parallel for
+ // #endif /* _OPENMP */
- /* (Requies the armarx::DebugObserverComponentPluginUser.)
- // Use the debug observer to log data over time.
- // The data can be viewed in the ObserverView and the LivePlotter.
- // (Before starting any threads, we don't need to lock mutexes.)
+ for (int i = 0; i < static_cast<int>(properties.nHumans); ++i)
{
- setDebugObserverDatafield("numBoxes", properties.numBoxes);
- setDebugObserverDatafield("boxLayerName", properties.boxLayerName);
- sendDebugObserverBatch();
- }
- */
+ // RVO::Vector2 goalVector = goals[i] - simulator->getAgentPosition(i);
- /* (Requires the armarx::ArVizComponentPluginUser.)
- // Draw boxes in ArViz.
- // (Before starting any threads, we don't need to lock mutexes.)
- drawBoxes(properties, arviz);
- */
+ // if (RVO::absSq(goalVector) > 1.0F)
+ // {
+ // goalVector = RVO::normalize(goalVector);
+ // }
- /* (Requires the armarx::LightweightRemoteGuiComponentPluginUser.)
- // Setup the remote GUI.
- {
- createRemoteGuiTab();
- RemoteGui_startRunningTask();
- }
- */
+ const auto agentPosition = simulator->getAgentPosition(i);
+ auto& simulatedHuman = simulatedHumans.at(i);
+
+ core::Pose2D pose = core::Pose2D::Identity();
+ pose.translation().x() = agentPosition.x() * 1000; // [m] -> [mm]
+ pose.translation().y() = agentPosition.y() * 1000; // [m] -> [mm]
+
+ simulatedHuman.reinitialize(pose);
+
+ const std::vector<core::Position> trajectoryPoints =
+ simulatedHuman.trajectory().positions();
+
+ const std::size_t lookAheadGoalIdx = std::min(trajectoryPoints.size() - 1, 5ul);
+ const auto& intermediateGoal = trajectoryPoints.at(lookAheadGoalIdx);
+ const Eigen::Vector2f rvoGoalHelper = intermediateGoal.head<2>() / 1000; // [mm] -> [m]
- ///
+ const RVO::Vector2 rvoGoal(rvoGoalHelper.x(), rvoGoalHelper.y());
+ RVO::Vector2 goalVector = rvoGoal - agentPosition;
+
+ if (RVO::absSq(goalVector) > 1.0F)
+ {
+ goalVector = RVO::normalize(goalVector);
+ }
+
+ simulator->setAgentPrefVelocity(i, goalVector);
+ }
+ }
+
+
+ void
+ Component::onConnectComponent()
+ {
//
// Costmaps
//
@@ -166,18 +197,120 @@ namespace armarx::navigation::components::human_simulator
};
}();
- /// create humans
+ /// create simulation environment
+ {
+ ARMARX_INFO << "Creating sim";
- simulatedHumans.clear();
+ simulator.setTimeStep(static_cast<float>(properties.taskPeriodMs) / 1000);
+ simulator.setAgentDefaults(15.0F, 10U, 1.5F, .1F, .4F, .5F);
- for (std::size_t i = 0; i < properties.nHumans; i++)
- {
- simulatedHumans.emplace_back(costmap);
+ const objpose::ObjectPoseSeq obstacles =
+ ObjectPoseClientPluginUser::getObjectPosesByProvider(
+ "R003_grasping_challenge_robot_placement_with_ivt_obstacles");
+
+ const auto objects = armarx::navigation::util::asSceneObjects(obstacles);
+
+
+ ARMARX_VERBOSE << obstacles.size() << " obstacles";
+
+ {
+ auto layer = arviz.layer("obstacles");
+
+
+ for (const auto& obst : objects->getCollisionModels())
+ {
+
+ ARMARX_INFO << "adding obstacle ...";
+ const VirtualRobot::BoundingBox bbox = obst->getGlobalBoundingBox();
+
+ Eigen::Vector2f bbMin = bbox.getMin().head<2>();
+ Eigen::Vector2f bbMax = bbox.getMax().head<2>();
+
+ // add some margin to the obstacles
+ const float margin = 100; // [mm]
+
+ bbMin.x() -= margin;
+ bbMin.y() -= margin;
+ bbMax.x() += margin;
+ bbMax.y() += margin;
+
+ ARMARX_VERBOSE << "Bounding box: (" << bbMin.transpose() << "), ("
+ << bbMax.transpose() << ")";
+
+ {
+ constexpr float zHeight = 3; // [mm]
+
+ viz::Polygon polygon(std::to_string(layer.size()));
+ polygon.addPoint(Eigen::Vector3f{bbMin.x(), bbMin.y(), zHeight});
+ polygon.addPoint(Eigen::Vector3f{bbMax.x(), bbMin.y(), zHeight});
+ polygon.addPoint(Eigen::Vector3f{bbMax.x(), bbMax.y(), zHeight});
+ polygon.addPoint(Eigen::Vector3f{bbMin.x(), bbMax.y(), zHeight});
+
+ polygon.color(simox::Color::black());
+ polygon.color(simox::Color::black());
+
+ layer.add(polygon);
+ }
+
+
+ const Eigen::Vector2d min = bbMin.cast<double>() / 1000; // [mm] -> [m]
+ const Eigen::Vector2d max = bbMax.cast<double>() / 1000; // [mm] -> [m]
+
+ std::vector<RVO::Vector2> obstacle;
+ obstacle.emplace_back(min.x(), min.y());
+ obstacle.emplace_back(max.x(), min.y());
+ obstacle.emplace_back(max.x(), max.y());
+ obstacle.emplace_back(min.x(), max.y());
+
+ simulator.addObstacle(obstacle);
+ }
+
+ arviz.commit(layer);
+ }
+
+ simulator.processObstacles();
}
+ /// create humans in simulation
+ {
+ simulatedHumans.clear();
+
+ for (std::size_t i = 0U; i < properties.nHumans; ++i)
+ {
+ armarx::navigation::human::simulation::SimulatedHuman& human =
+ simulatedHumans.emplace_back(costmap);
- ///
+ const RVO::Vector2 position =
+ RVO::Vector2(human.human().pose.translation().x() / 1000,
+ human.human().pose.translation().y() / 1000);
+ auto goal = human.trajectory().positions().back() / 1000; // [mm] -> [m]
+ ARMARX_VERBOSE << "Adding agent";
+ // simulator.addAgent(position, simulator.addGoal(hrvo::Vector2(goal.x(), goal.y())));
+ simulator.addAgent(
+ position); // , simulator.addGoal(hrvo::Vector2(goal.x(), goal.y())));
+
+ goals.emplace_back(goal.x(), goal.y());
+ }
+ }
+
+ /// load robot and add it to the simulator
+ {
+ robot = virtualRobotReaderPlugin->get().getRobot(properties.robotName);
+ ARMARX_CHECK(
+ virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
+
+ const auto robotPosH = robot->getGlobalPosition();
+ const RVO::Vector2 robotPos(robotPosH.x() / 1000, robotPosH.y() / 1000);
+
+ robotAgentId = simulator.addAgent(robotPos);
+ simulator.setAgentMaxSpeed(robotAgentId,
+ 0); // we do not allow the robot to be perturbed
+ simulator.setAgentPrefVelocity(robotAgentId, RVO::Vector2(0, 0));
+ simulator.setAgentRadius(robotAgentId, 0.6);
+ }
+
+ /// start task
task = new PeriodicTask<Component>(
this, &Component::runPeriodically, properties.taskPeriodMs, false, "runningTask");
task->start();
@@ -213,25 +346,58 @@ namespace armarx::navigation::components::human_simulator
void
Component::runPeriodically()
{
- // obtain data from perception
-
const DateTime timestamp = Clock::Now();
- //
- // Robot
- //
+ setPreferredVelocities(&simulator, goals);
- // ARMARX_CHECK(virtualRobotReaderPlugin->get().synchronizeRobot(*robot, timestamp));
- // const core::Pose global_T_robot(robot->getGlobalPose());
+ // TODO(fabian.reister): check how much the agents interact => this can be an evaluation criterion
- // ARMARX_INFO << "Robot position: " << global_T_robot.translation().head<2>();
+ // update robot
+ {
+ ARMARX_CHECK(
+ virtualRobotReaderPlugin->get().synchronizeRobot(*robot, armarx::Clock::Now()));
+ const auto robotPosH = robot->getGlobalPosition();
+ const RVO::Vector2 robotPos(robotPosH.x() / 1000, robotPosH.y() / 1000);
- human::Humans humans;
+ simulator.setAgentPosition(robotAgentId, robotPos);
+ }
+
+ simulator.doStep();
+
+ // visualize paths
+ {
+ auto layer = arviz.layer("trajectories");
+ for (const auto& simulatedHuman : simulatedHumans)
+ {
+ viz::Path path(std::to_string(layer.size()));
+
+ for (const auto& pt : simulatedHuman.trajectory().points())
+ {
+ path.addPoint(pt.waypoint.pose.translation());
+ }
- for (auto& simulatedHuman : simulatedHumans)
+ // unique and distinct color for this human
+ path.colorGlasbeyLUT(layer.size());
+
+ layer.add(path);
+ }
+
+ arviz.commit(layer);
+ }
+
+
+ human::Humans humans;
+ for (std::size_t i = 0; i < properties.nHumans; i++)
{
- humans.push_back(simulatedHuman.update());
+ const auto pos = simulator.getAgentPosition(i);
+
+ human::Human human;
+ human.pose.setIdentity();
+ human.pose.translation().x() = pos.x() * 1000; // [m] -> [mm]
+ human.pose.translation().y() = pos.y() * 1000; // [m] -> [mm]
+
+ humans.push_back(human);
}
ARMARX_VERBOSE << "Updating humans.";
diff --git a/source/armarx/navigation/components/human_simulator/Component.h b/source/armarx/navigation/components/human_simulator/Component.h
index 52e7aadfee802ea9034d0f50d14195782118345e..7235893d36fcd1661a82bb63123f60e85ccd2d9e 100644
--- a/source/armarx/navigation/components/human_simulator/Component.h
+++ b/source/armarx/navigation/components/human_simulator/Component.h
@@ -26,33 +26,38 @@
// #include <mutex>
+#include <string>
+#include <VirtualRobot/VirtualRobot.h>
#include "ArmarXCore/core/services/tasks/PeriodicTask.h"
#include <ArmarXCore/core/Component.h>
-#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
+#include "RobotAPI/libraries/ArmarXObjects/plugins/ObjectPoseClientPlugin.h"
+#include "RobotAPI/libraries/armem/client/plugins/ReaderWriterPlugin.h"
#include "RobotAPI/libraries/armem_robot_state/client/common/VirtualRobotReader.h"
// #include <ArmarXCore/libraries/ArmarXCoreComponentPlugins/DebugObserverComponentPlugin.h>
// #include <ArmarXGui/libraries/ArmarXGuiComponentPlugins/LightweightRemoteGuiComponentPlugin.h>
-// #include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
+#include <RobotAPI/libraries/RobotAPIComponentPlugins/ArVizComponentPlugin.h>
-#include "armarx/navigation/simulation/SimulatedHuman.h"
#include "armarx/navigation/memory/client/costmap/Reader.h"
#include "armarx/navigation/memory/client/human/Writer.h"
+#include "armarx/navigation/simulation/SimulatedHuman.h"
+// #include <Simulator.h>
+#include "RVO.h"
#include <armarx/navigation/components/human_simulator/ComponentInterface.h>
-
namespace armarx::navigation::components::human_simulator
{
class Component :
virtual public armarx::Component,
- virtual public armarx::navigation::components::human_simulator::ComponentInterface
- // , virtual public armarx::DebugObserverComponentPluginUser
- // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
- // , virtual public armarx::ArVizComponentPluginUser
+ virtual public armarx::navigation::components::human_simulator::ComponentInterface,
+ virtual public armarx::ObjectPoseClientPluginUser,
+ // , virtual public armarx::DebugObserverComponentPluginUser
+ // , virtual public armarx::LightweightRemoteGuiComponentPluginUser
+ virtual public armarx::ArVizComponentPluginUser
{
public:
Component();
@@ -106,6 +111,8 @@ namespace armarx::navigation::components::human_simulator
void runPeriodically();
+ void setPreferredVelocities(RVO::RVOSimulator* simulator,
+ const std::vector<RVO::Vector2>& goals);
private:
@@ -114,7 +121,6 @@ namespace armarx::navigation::components::human_simulator
PeriodicTask<Component>::pointer_type task;
-
// Private member variables go here.
@@ -127,6 +133,8 @@ namespace armarx::navigation::components::human_simulator
std::size_t nHumans = 4;
human::simulation::SimulatedHumanParams humanParams;
+
+ std::string robotName = "Armar6";
};
Properties properties;
/* Use a mutex if you access variables from different threads
@@ -157,6 +165,9 @@ namespace armarx::navigation::components::human_simulator
template <typename T>
using ReaderWriterPlugin = armem::client::plugins::ReaderWriterPlugin<T>;
+ VirtualRobot::RobotPtr robot;
+
+ std::size_t robotAgentId;
ReaderWriterPlugin<armem::robot_state::VirtualRobotReader>* virtualRobotReaderPlugin =
nullptr;
@@ -166,10 +177,13 @@ namespace armarx::navigation::components::human_simulator
ReaderWriterPlugin<memory::client::human::Writer>* humanWriterPlugin = nullptr;
-
+
std::vector<human::simulation::SimulatedHuman> simulatedHumans;
+ // hrvo::Simulator simulator;
+ RVO::RVOSimulator simulator;
+ std::vector<RVO::Vector2> goals;
};
} // namespace armarx::navigation::components::human_simulator
diff --git a/source/armarx/navigation/simulation/SimulatedHuman.cpp b/source/armarx/navigation/simulation/SimulatedHuman.cpp
index 0a4d735dff8efc31ca6e49c987568af159139b24..857ad24282a527fc40fe436a9c1a7703a41d7c04 100644
--- a/source/armarx/navigation/simulation/SimulatedHuman.cpp
+++ b/source/armarx/navigation/simulation/SimulatedHuman.cpp
@@ -29,6 +29,7 @@
#include "armarx/navigation/algorithms/util.h"
#include "armarx/navigation/conversions/eigen.h"
#include "armarx/navigation/core/StaticScene.h"
+#include "armarx/navigation/core/basic_types.h"
#include "armarx/navigation/core/types.h"
#include "armarx/navigation/global_planning/SPFA.h"
#include "armarx/navigation/human/types.h"
@@ -53,10 +54,7 @@ namespace armarx::navigation::human::simulation
step();
- const Eigen::Vector2f goal =
- conv::to2D(globalTrajectory_.points().back().waypoint.pose.translation());
-
- if ((goal - human_.pose.translation()).norm() < params_.goalDistanceThreshold)
+ if (goalReached())
{
ARMARX_INFO << "Human reached goal";
state_ = State::GoalReached;
@@ -75,6 +73,31 @@ namespace armarx::navigation::human::simulation
return human_;
}
+ bool
+ SimulatedHuman::goalReached() const
+ {
+ const Eigen::Vector2f goal =
+ conv::to2D(globalTrajectory_.points().back().waypoint.pose.translation());
+
+ return ((goal - human_.pose.translation()).norm() < params_.goalDistanceThreshold);
+ }
+
+ void
+ SimulatedHuman::reinitialize(const core::Pose2D& pose)
+ {
+ human_.pose = pose;
+
+ if(goalReached())
+ {
+ ARMARX_VERBOSE << "Goal reached";
+ state_ = State::Idle;
+ }
+
+ initialize();
+ update();
+
+ }
+
void
SimulatedHuman::initialize()
{
@@ -88,14 +111,34 @@ namespace armarx::navigation::human::simulation
global_planning::SPFA planner(params, scene);
- const auto start = human_.pose;
+ const core::Pose2D start = human_.pose;
- while (true)
+ if (state_ == State::Idle) // human reached goal and needs a new one
{
- const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
- ARMARX_CHECK(sampledPose.has_value());
- const auto& goal = sampledPose.value();
+ while (true)
+ {
+ const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
+ ARMARX_CHECK(sampledPose.has_value());
+ const core::Pose2D& goal = sampledPose.value();
+
+
+ const auto plan = planner.plan(conv::to3D(start), conv::to3D(goal));
+
+ // check if plan could be created. otherwise try another goal
+ if (plan.has_value())
+ {
+ globalTrajectory_ = plan->trajectory;
+ human_ = human::Human{.pose = start,
+ .linearVelocity = Eigen::Vector2f::Zero(),
+ .detectionTime = Clock::Now()};
+ return;
+ }
+ }
+ }
+ else if (state_ == State::Walking) // human is walking and replans to existing goal
+ {
+ const auto goal = conv::to2D(globalTrajectory_.poses().back());
const auto plan = planner.plan(conv::to3D(start), conv::to3D(goal));
// check if plan could be created. otherwise try another goal
@@ -148,5 +191,7 @@ namespace armarx::navigation::human::simulation
const auto sampledPose = algorithms::sampleValidPositionInMap(distanceField_);
ARMARX_CHECK(sampledPose.has_value());
human_.pose = sampledPose.value();
+
+ update(); // should bring the robot into walking state
}
} // namespace armarx::navigation::human::simulation
diff --git a/source/armarx/navigation/simulation/SimulatedHuman.h b/source/armarx/navigation/simulation/SimulatedHuman.h
index fa4ed6a8a01a748937c8aa4a57b5f9185705cbe7..b1bdc55658c24755f6094b45cdbd40e9ac017f18 100644
--- a/source/armarx/navigation/simulation/SimulatedHuman.h
+++ b/source/armarx/navigation/simulation/SimulatedHuman.h
@@ -31,10 +31,10 @@ namespace armarx::navigation::human::simulation
struct SimulatedHumanParams
{
- float goalDistanceThreshold = 100;
+ float goalDistanceThreshold = 100; // [mm]
- float minLinearVelocity = 100;
- float maxLinearVelocity = 200;
+ float minLinearVelocity = 100; // [mm/s]
+ float maxLinearVelocity = 200; // [mm/s]
};
class SimulatedHuman
@@ -46,6 +46,34 @@ namespace armarx::navigation::human::simulation
Human update();
+
+ human::Human&
+ human()
+ {
+ return human_;
+ }
+
+ core::GlobalTrajectory&
+ trajectory()
+ {
+ return globalTrajectory_;
+ }
+
+ const core::GlobalTrajectory&
+ trajectory() const
+ {
+ return globalTrajectory_;
+ }
+
+ bool goalReached() const;
+
+ /**
+ * @brief resets the human to the given pose and replans the global optimal trajectory
+ *
+ * @param pose
+ */
+ void reinitialize(const core::Pose2D& pose);
+
protected:
void initialize();
@@ -58,6 +86,7 @@ namespace armarx::navigation::human::simulation
GoalReached
};
+
private:
const algorithms::Costmap distanceField_;