Feature/sm dancebot ue

smacc2/include/smacc2/client_base_components/cp_topic_subscriber.hpp

@@ -44,19 +44,21 @@ class CpTopicSubscriber : public smacc2::ISmaccComponent
 
   virtual ~CpTopicSubscriber() {}
 
-  smacc2::SmaccSignal<void(const MessageType &)> onFirstMessageReceived_;
-  smacc2::SmaccSignal<void(const MessageType &)> onMessageReceived_;
-
   std::function<void(const MessageType &)> postMessageEvent;
   std::function<void(const MessageType &)> postInitialMessageEvent;
 
+  smacc2::SmaccSignal<void(const MessageType &)> onFirstMessageReceived_;
+  smacc2::SmaccSignal<void(const MessageType &)> onMessageReceived_;
+
+  // signal subscription method. This signal will be triggered when the first message is received
   template <typename T>
   boost::signals2::connection onMessageReceived(
     void (T::*callback)(const MessageType &), T * object)
   {
     return this->getStateMachine()->createSignalConnection(onMessageReceived_, callback, object);
   }
 
+  // signal subscription method. This signal will be triggered when the first message is received
   template <typename T>
   boost::signals2::connection onFirstMessageReceived(
     void (T::*callback)(const MessageType &), T * object)

smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp

@@ -216,7 +216,7 @@ template <typename EventType>
 void ISmaccStateMachine::postEvent(EventLifeTime evlifetime)
 {
   auto evname = smacc2::introspection::demangleSymbol<EventType>();
-  RCLCPP_DEBUG_STREAM(getLogger(), "Event " << evname);
+  RCLCPP_INFO_STREAM(getLogger(), "Event " << evname);
   auto * ev = new EventType();
   this->postEvent(ev, evlifetime);
 }

smacc2_client_library/nav2z_client/nav2z_client/CMakeLists.txt

@@ -24,6 +24,7 @@ find_package(tf2_geometry_msgs REQUIRED)
 find_package(pluginlib REQUIRED)
 find_package(bond REQUIRED)
 find_package(slam_toolbox REQUIRED)
+find_package(ament_index_cpp REQUIRED)
 
 set(dependencies
   smacc2
@@ -37,6 +38,7 @@ set(dependencies
   tf2_geometry_msgs
   bond
   slam_toolbox
+  ament_index_cpp
 )
 
 include_directories(include)

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator.hpp

@@ -24,23 +24,14 @@
 #include <smacc2/smacc.hpp>
 
 #include <geometry_msgs/msg/pose.hpp>
+#include <nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp>
 
 namespace cl_nav2z
 {
 class ClNav2Z;
 
-struct Pose2D
+struct EvWaypointFinal : sc::event<EvWaypointFinal>
 {
-  Pose2D(double x, double y, double yaw)
-  {
-    this->x_ = x;
-    this->y_ = y;
-    this->yaw_ = yaw;
-  }
-
-  double x_;
-  double y_;
-  double yaw_;
 };
 
 struct NavigateNextWaypointOptions
@@ -52,11 +43,9 @@ struct NavigateNextWaypointOptions
 // This component contains a list of waypoints. These waypoints can
 // be iterated in the different states using CbNextWaiPoint
 // waypoint index is only incremented if the current waypoint is successfully reached
-class CpWaypointNavigator : public smacc2::ISmaccComponent
+class CpWaypointNavigator : public CpWaypointNavigatorBase
 {
 public:
-  WaypointEventDispatcher waypointsEventDispatcher;
-
   ClNav2Z * client_;
 
   CpWaypointNavigator();
@@ -69,14 +58,6 @@ class CpWaypointNavigator : public smacc2::ISmaccComponent
     waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client_);
   }
 
-  void loadWayPointsFromFile(std::string filepath);
-
-  void loadWayPointsFromFile2(std::string filepath);
-
-  void setWaypoints(const std::vector<geometry_msgs::msg::Pose> & waypoints);
-
-  void setWaypoints(const std::vector<Pose2D> & waypoints);
-
   std::optional<std::shared_future<
     std::shared_ptr<rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose> > > >
   sendNextGoal(
@@ -86,39 +67,17 @@ class CpWaypointNavigator : public smacc2::ISmaccComponent
 
   void stopWaitingResult();
 
-  const std::vector<geometry_msgs::msg::Pose> & getWaypoints() const;
-  const std::vector<std::string> & getWaypointNames() const;
-  std::optional<geometry_msgs::msg::Pose> getNamedPose(std::string name) const;
-
-  long getCurrentWaypointIndex() const;
-  std::optional<std::string> getCurrentWaypointName() const;
-
-  long currentWaypoint_;
-
-  void rewind(int count);
-
-  void forward(int count);
-  void seekName(std::string name);
-
   smacc2::SmaccSignal<void()> onNavigationRequestSucceded;
   smacc2::SmaccSignal<void()> onNavigationRequestAborted;
   smacc2::SmaccSignal<void()> onNavigationRequestCancelled;
 
 private:
-  void insertWaypoint(int index, geometry_msgs::msg::Pose & newpose);
-
-  void removeWaypoint(int index);
-
   void onNavigationResult(const ClNav2Z::WrappedResult & r);
 
   void onGoalReached(const ClNav2Z::WrappedResult & res);
   void onGoalCancelled(const ClNav2Z::WrappedResult & /*res*/);
   void onGoalAborted(const ClNav2Z::WrappedResult & /*res*/);
 
-  std::vector<geometry_msgs::msg::Pose> waypoints_;
-
-  std::vector<std::string> waypointsNames_;
-
   boost::signals2::connection succeddedNav2ZClientConnection_;
   boost::signals2::connection abortedNav2ZClientConnection_;
   boost::signals2::connection cancelledNav2ZClientConnection_;

smacc2_client_library/nav2z_client/nav2z_client/include/nav2z_client/components/waypoints_navigator/cp_waypoints_navigator_base.hpp

@@ -0,0 +1,102 @@
+// Copyright 2021 RobosoftAI Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/*****************************************************************************************************************
+ *
+ * 	 Authors: Pablo Inigo Blasco, Brett Aldrich
+ *
+ ******************************************************************************************************************/
+#pragma once
+
+#include <nav2z_client/components/waypoints_navigator/cp_waypoints_event_dispatcher.hpp>
+#include <nav2z_client/nav2z_client.hpp>
+#include <smacc2/smacc.hpp>
+
+#include <geometry_msgs/msg/pose.hpp>
+
+namespace cl_nav2z
+{
+struct Pose2D
+{
+  Pose2D(double x, double y, double yaw)
+  {
+    this->x_ = x;
+    this->y_ = y;
+    this->yaw_ = yaw;
+  }
+
+  double x_;
+  double y_;
+  double yaw_;
+};
+
+// This component contains a list of waypoints. These waypoints can
+// be iterated in the different states using CbNextWaiPoint
+// waypoint index is only incremented if the current waypoint is successfully reached
+class CpWaypointNavigatorBase : public smacc2::ISmaccComponent
+{
+public:
+  WaypointEventDispatcher waypointsEventDispatcher;
+
+  CpWaypointNavigatorBase();
+
+  virtual ~CpWaypointNavigatorBase();
+
+  void onInitialize() override;
+
+  // template <typename TOrthogonal, typename TSourceObject>
+  // void onOrthogonalAllocation()
+  // {
+  //   waypointsEventDispatcher.initialize<TSourceObject, TOrthogonal>(client_);
+  // }
+
+  void loadWayPointsFromFile(std::string filepath);
+
+  void loadWayPointsFromFile2(std::string filepath);
+
+  void setWaypoints(const std::vector<geometry_msgs::msg::Pose> & waypoints);
+
+  void setWaypoints(const std::vector<Pose2D> & waypoints);
+
+  const std::vector<geometry_msgs::msg::Pose> & getWaypoints() const;
+  const std::vector<std::string> & getWaypointNames() const;
+  std::optional<geometry_msgs::msg::Pose> getNamedPose(std::string name) const;
+  geometry_msgs::msg::Pose getPose(int index) const;
+  geometry_msgs::msg::Pose getCurrentPose() const;
+
+  long getCurrentWaypointIndex() const;
+  std::optional<std::string> getCurrentWaypointName() const;
+
+  long currentWaypoint_;
+
+  void rewind(int count);
+
+  void forward(int count);
+  void seekName(std::string name);
+
+  void loadWaypointsFromYamlParameter(
+    std::string parameter_name, std::string yaml_file_package_name);
+
+  void notifyGoalReached();
+
+protected:
+  void insertWaypoint(int index, geometry_msgs::msg::Pose & newpose);
+
+  void removeWaypoint(int index);
+
+  std::vector<geometry_msgs::msg::Pose> waypoints_;
+
+  std::vector<std::string> waypointsNames_;
+};
+}  // namespace cl_nav2z

smacc2_client_library/nav2z_client/nav2z_client/package.xml

@@ -24,6 +24,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>bond</depend>
   <depend>slam_toolbox</depend>
+  <depend>ament_index_cpp</depend>
 
   <export>
     <build_type>ament_cmake</build_type>

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_absolute_rotate.cpp

@@ -82,6 +82,7 @@ void CbAbsoluteRotate::onEntry()
     auto pathname = this->getCurrentState()->getName() + " - " + getName();
     odomTracker_->pushPath(pathname);
     odomTracker_->setStartPoint(p->toPoseStampedMsg());
+    odomTracker_->setCurrentMotionGoal(goal.pose);
     odomTracker_->setWorkingMode(odom_tracker::WorkingMode::RECORD_PATH);
   }
 

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_nav2z_client_behavior_base.cpp

@@ -80,6 +80,9 @@ bool CbNav2ZClientBehaviorBase::isOwnActionResponse(const ClNav2Z::WrappedResult
 
 void CbNav2ZClientBehaviorBase::onNavigationResult(const ClNav2Z::WrappedResult & r)
 {
+  RCLCPP_DEBUG(
+    getLogger(), "[%s] Received result event from action server, result code", getName().c_str());
+
   if (r.code == rclcpp_action::ResultCode::SUCCEEDED)
   {
     this->onNavigationActionSuccess(r);

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_forward.cpp

@@ -68,15 +68,13 @@ void CbNavigateForward::onEntry()
   auto p = nav2zClient_->getComponent<Pose>();
   auto referenceFrame = p->getReferenceFrame();
   auto currentPoseMsg = p->toPoseMsg();
-  tf2::Transform currentPose;
-  tf2::fromMsg(currentPoseMsg, currentPose);
 
   RCLCPP_INFO_STREAM(
     getLogger(), "[" << getName() << "]"
                      << "current pose: " << currentPoseMsg);
 
   // force global orientation if it is requested
-  if (options.forwardSpeed)
+  if (options.forceInitialOrientation)
   {
     currentPoseMsg.orientation = *(options.forceInitialOrientation);
     RCLCPP_WARN_STREAM(
@@ -85,6 +83,9 @@ void CbNavigateForward::onEntry()
           << "Forcing initial straight motion orientation: " << currentPoseMsg.orientation);
   }
 
+  tf2::Transform currentPose;
+  tf2::fromMsg(currentPoseMsg, currentPose);
+
   tf2::Transform targetPose;
   if (goalPose_)
   {