GoalReachedMonitor.cpp
Go to the documentation of this file.
1 #include "GoalReachedMonitor.h"
2 
3 #include <VirtualRobot/Robot.h> // IWYU pragma: keep
4 
7 
10 
12 {
13 
15  const core::Scene& scene,
16  const GoalReachedMonitorConfig& config) :
17  goal_(goal), scene(scene), config(config), reachedCount(0)
18  {
19  //ARMARX_CHECK(not waypoints.empty());
20  }
21 
22  bool
23  GoalReachedMonitor::goalReached(const bool filter) const noexcept
24  {
25  // return false;
27 
28  const core::Pose world_T_robot(scene.robot->getGlobalPose());
29  const core::Pose& world_T_goal(goal_);
30 
31  const core::Pose robot_T_goal = world_T_robot.inverse() * world_T_goal;
32 
33  ARMARX_VERBOSE << "Position error: " << robot_T_goal.translation().head<2>().norm();
34 
35  const bool posReached = robot_T_goal.translation().head<2>().norm() <= config.posTh;
36  const bool oriReached = Eigen::AngleAxisf(robot_T_goal.linear()).angle() <= config.oriTh;
37 
38  if (posReached)
39  {
40  ARMARX_DEBUG << "Position goal `" << robot_T_goal.translation().head<2>()
41  << "` reached";
42  }
43 
44  if (oriReached)
45  {
46  ARMARX_DEBUG << "Orientation goal reached";
47  }
48 
49  const bool linearVelocityLow = scene.platformVelocity.linear.norm() < config.linearVelTh;
50  const bool angularVelocityLow = scene.platformVelocity.angular.norm() < config.angularVelTh;
51 
52  const bool velocityLow = linearVelocityLow and angularVelocityLow;
53 
54  if (velocityLow)
55  {
56  ARMARX_DEBUG << "Robot has come to rest.";
57  }
58 
59  bool reached = posReached and oriReached and velocityLow;
60 
61  if (reached)
62  {
63  reachedCount++;
64  ARMARX_DEBUG << "Goal reached! reachedCount: " << reachedCount;
65  return !filter || (reachedCount >= config.filterCount);
66  }
67  else
68  {
69  reachedCount = 0;
70  return false;
71  }
72  }
73 
74  const core::Pose&
75  GoalReachedMonitor::goal() const noexcept
76  {
77  return goal_;
78  }
79 
81  goal_{other.goal_}, scene{other.scene}, config(other.config)
82  {
83  }
84 
85  GoalReachedMonitor&
87  {
88  return *this;
89  }
90 } // namespace armarx::navigation::server
ARMARX_VERBOSE
#define ARMARX_VERBOSE
Definition: Logging.h:187
armarx::navigation::core::Pose
Eigen::Isometry3f Pose
Definition: basic_types.h:31
basic_types.h
armarx::navigation::server::GoalReachedMonitor::goal
const core::Pose & goal() const noexcept
Definition: GoalReachedMonitor.cpp:75
armarx::navigation::server::GoalReachedMonitor::GoalReachedMonitor
GoalReachedMonitor(const core::Pose &goal, const core::Scene &scene, const GoalReachedMonitorConfig &config)
Definition: GoalReachedMonitor.cpp:14
GoalReachedMonitor.h
trace.h
armarx::navigation::server
This file is part of ArmarX.
Definition: EventPublishingInterface.h:6
armarx::navigation::server::GoalReachedMonitor
Definition: GoalReachedMonitor.h:45
ARMARX_TRACE
#define ARMARX_TRACE
Definition: trace.h:77
armarx::navigation::server::GoalReachedMonitor::goalReached
bool goalReached(bool filter=true) const noexcept
Definition: GoalReachedMonitor.cpp:23
armarx::navigation::server::GoalReachedMonitor::operator=
GoalReachedMonitor & operator=(GoalReachedMonitor &&) noexcept
Definition: GoalReachedMonitor.cpp:86
ARMARX_DEBUG
#define ARMARX_DEBUG
Definition: Logging.h:184
armarx::navigation::server::GoalReachedMonitorConfig
Definition: GoalReachedMonitor.h:34
armarx::navigation::core::Scene
Definition: types.h:60
Logging.h
types.h
norm
double norm(const Point &a)
Definition: point.hpp:102