fixing navigation galactic and humble I (#509)

* changes

* format minor

README.md

@@ -2,7 +2,7 @@
 
 SMACC2 is an event-driven, asynchronous, behavioral state machine library for real-time ROS 2 (Robotic Operating System) applications written in C++, designed to allow programmers to build robot control applications for multicomponent robots, in an intuitive and systematic manner.
 
-SMACC was inspired by Harel's statecharts and the [SMACH ROS package](http://wiki.ros.org/smach). SMACC is built on top of the [Boost StateChart library](https://www.boost.org/doc/libs/1_53_0/libs/statechart/doc/index.html). 
+SMACC was inspired by Harel's statecharts and the [SMACH ROS package](http://wiki.ros.org/smach). SMACC is built on top of the [Boost StateChart library](https://www.boost.org/doc/libs/1_53_0/libs/statechart/doc/index.html).
 
 ## Repository Status, Packages and Documentation
 

smacc2/include/smacc2/client_bases/smacc_action_client_base.hpp

@@ -289,7 +289,9 @@ class SmaccActionClientBase : public ISmaccActionClient
 
       if (resultCallbackPtr != nullptr)
       {
-        RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "]  Result CB calling user callback");
+        RCLCPP_INFO_STREAM(
+          getLogger(), "[" << getName() << "]  Result CB calling user callback:"
+                           << demangleSymbol(typeid(*resultCallbackPtr).name()));
         (*resultCallbackPtr)(result);
       }
       else

smacc2/include/smacc2/impl/smacc_state_machine_impl.hpp

@@ -215,7 +215,7 @@ template <typename EventType>
 void ISmaccStateMachine::postEvent(EventLifeTime evlifetime)
 {
   auto evname = smacc2::introspection::demangleSymbol<EventType>();
-  RCLCPP_INFO_STREAM(getLogger(), "Event " << evname);
+  // RCLCPP_INFO_STREAM(getLogger(), "Event " << evname);
   auto * ev = new EventType();
   this->postEvent(ev, evlifetime);
 }
@@ -432,7 +432,15 @@ boost::signals2::connection ISmaccStateMachine::createSignalConnection(
     std::is_base_of<ISmaccOrthogonal, TSmaccObjectType>::value ||
     std::is_base_of<ISmaccStateMachine, TSmaccObjectType>::value)
   {
+    RCLCPP_INFO(
+      getLogger(),
+      "[StateMachine] long life-time smacc signal subscription created. Subscriber is %s. Callback "
+      "is: %s",
+      demangledTypeName<TSmaccObjectType>().c_str(),
+      demangleSymbol(typeid(callback).name()).c_str());
+
     connection = binder.bindaux(signal, callback, object, nullptr);
+    longLivedSignalConnections_.push_back(connection);
   }
   else if (
     std::is_base_of<ISmaccState, TSmaccObjectType>::value ||

smacc2/include/smacc2/smacc_state_machine.hpp

@@ -194,6 +194,8 @@ class ISmaccStateMachine
   // orthogonals
   std::map<std::string, std::shared_ptr<smacc2::ISmaccOrthogonal>> orthogonals_;
 
+  std::vector<boost::signals2::scoped_connection> longLivedSignalConnections_;
+
 protected:
   std::shared_ptr<SmaccStateMachineInfo> stateMachineInfo_;
 

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

@@ -109,9 +109,11 @@ class WaypointNavigator : public smacc2::ISmaccComponent
 
   void removeWaypoint(int index);
 
-  void onGoalReached(ClNav2Z::WrappedResult & res);
-  void onGoalCancelled(ClNav2Z::WrappedResult & /*res*/);
-  void onGoalAborted(ClNav2Z::WrappedResult & /*res*/);
+  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_;
 

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

@@ -68,7 +68,7 @@ void CbAbsoluteRotate::onEntry()
 
   ClNav2Z::Goal goal;
   goal.pose.header.frame_id = referenceFrame;
-  goal.pose.header.stamp = getNode()->now();
+  //goal.pose.header.stamp = getNode()->now();
 
   auto targetAngle = goal_angle * M_PI / 180.0;
   goal.pose.pose.position = currentPoseMsg.position;

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

@@ -25,6 +25,8 @@ CbNav2ZClientBehaviorBase::~CbNav2ZClientBehaviorBase() {}
 
 void CbNav2ZClientBehaviorBase::sendGoal(ClNav2Z::Goal & goal)
 {
+  RCLCPP_INFO_STREAM(getLogger(), "[" << getName() << "] Creating sending goal callback signal.");
+
   this->navigationCallback_ = std::make_shared<cl_nav2z::ClNav2Z::SmaccNavigateResultSignal>();
 
   this->getStateMachine()->createSignalConnection(

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_backward.cpp

@@ -63,7 +63,7 @@ void CbNavigateBackwards::onEntry()
 
   ClNav2Z::Goal goal;
   goal.pose.header.frame_id = referenceFrame;
-  goal.pose.header.stamp = getNode()->now();
+  //goal.pose.header.stamp = getNode()->now();
   tf2::toMsg(targetPose, goal.pose.pose);
   RCLCPP_INFO_STREAM(getLogger(), "[CbNavigateBackwards] TARGET POSE BACKWARDS: " << goal.pose);
 

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

@@ -113,7 +113,7 @@ void CbNavigateForward::onEntry()
   // action goal
   ClNav2Z::Goal goal;
   goal.pose.header.frame_id = referenceFrame;
-  goal.pose.header.stamp = getNode()->now();
+  //goal.pose.header.stamp = getNode()->now();
   tf2::toMsg(targetPose, goal.pose.pose);
   RCLCPP_INFO_STREAM(
     getLogger(), "[" << getName() << "]"
@@ -122,7 +122,9 @@ void CbNavigateForward::onEntry()
   // current pose
   geometry_msgs::msg::PoseStamped currentStampedPoseMsg;
   currentStampedPoseMsg.header.frame_id = referenceFrame;
-  currentStampedPoseMsg.header.stamp = getNode()->now();
+  currentStampedPoseMsg.header.stamp =
+    getNode()->now();  // probably it is better avoid setting that goal timestamp
+
   tf2::toMsg(currentPose, currentStampedPoseMsg.pose);
 
   odomTracker_ = nav2zClient_->getComponent<OdomTracker>();

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_navigate_global_position.cpp

@@ -89,7 +89,7 @@ void CbNavigateGlobalPosition::execute()
   RCLCPP_INFO(getLogger(), "Sending Goal to MoveBase");
   ClNav2Z::Goal goal;
   goal.pose.header.frame_id = referenceFrame;
-  goal.pose.header.stamp = getNode()->now();
+  //goal.pose.header.stamp = getNode()->now();
 
   goal.pose.pose.position = goalPosition;
   tf2::Quaternion q;

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_rotate.cpp

@@ -58,7 +58,7 @@ void CbRotate::onEntry()
   auto odomTracker = nav2zClient_->getComponent<odom_tracker::OdomTracker>();
   ClNav2Z::Goal goal;
   goal.pose.header.frame_id = referenceFrame;
-  goal.pose.header.stamp = getNode()->now();
+  //goal.pose.header.stamp = getNode()->now();
 
   auto currentAngle = tf2::getYaw(currentPoseMsg.orientation);
   auto targetAngle = currentAngle + angle_increment_degree * M_PI / 180.0;

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/client_behaviors/cb_undo_path_backwards.cpp

@@ -72,11 +72,15 @@ void CbUndoPathBackwards::onEntry()
   if (forwardpath.poses.size() > 0)
   {
     goal.pose = forwardpath.poses.front();
-    goal.pose.header.stamp = getNode()->now();
+    //goal.pose.header.stamp = getNode()->now();
+    goal.pose.header.stamp = rclcpp::Time(0);
 
-    if (options_->undoControllerName_)
+    if (options_ && options_->undoControllerName_)
     {
       plannerSwitcher->setUndoPathBackwardPlanner(false);
+      RCLCPP_INFO_STREAM(
+        getLogger(),
+        "[" << getName() << "] Undoing path with controller: " << *options_->undoControllerName_);
       plannerSwitcher->setDesiredController(*options_->undoControllerName_);
       plannerSwitcher->commitPublish();
     }

smacc2_client_library/nav2z_client/nav2z_client/src/nav2z_client/components/waypoints_navigator/waypoints_navigator.cpp

@@ -38,21 +38,21 @@ WaypointNavigator::WaypointNavigator() : currentWaypoint_(0), waypoints_(0) {}
 
 void WaypointNavigator::onInitialize() { client_ = dynamic_cast<ClNav2Z *>(owner_); }
 
-void WaypointNavigator::onGoalCancelled(ClNav2Z::WrappedResult & /*res*/)
+void WaypointNavigator::onGoalCancelled(const ClNav2Z::WrappedResult & /*res*/)
 {
   stopWaitingResult();
 
   this->onNavigationRequestCancelled();
 }
 
-void WaypointNavigator::onGoalAborted(ClNav2Z::WrappedResult & /*res*/)
+void WaypointNavigator::onGoalAborted(const ClNav2Z::WrappedResult & /*res*/)
 {
   stopWaitingResult();
 
   this->onNavigationRequestAborted();
 }
 
-void WaypointNavigator::onGoalReached(ClNav2Z::WrappedResult & /*res*/)
+void WaypointNavigator::onGoalReached(const ClNav2Z::WrappedResult & /*res*/)
 {
   waypointsEventDispatcher.postWaypointEvent(currentWaypoint_);
   currentWaypoint_++;
@@ -177,7 +177,7 @@ WaypointNavigator::sendNextGoal(
 
     // configuring goal
     goal.pose.header.frame_id = p->getReferenceFrame();
-    goal.pose.header.stamp = getNode()->now();
+    //goal.pose.header.stamp = getNode()->now();
     goal.pose.pose = next;
 
     auto plannerSwitcher = client_->getComponent<PlannerSwitcher>();
@@ -230,17 +230,22 @@ WaypointNavigator::sendNextGoal(
     }
 
     // SEND GOAL
-    if (!succeddedNav2ZClientConnection_.connected())
-    {
-      this->succeddedNav2ZClientConnection_ =
-        client_->onSucceeded(&WaypointNavigator::onGoalReached, this);
-      this->cancelledNav2ZClientConnection_ =
-        client_->onAborted(&WaypointNavigator::onGoalCancelled, this);
-      this->abortedNav2ZClientConnection_ =
-        client_->onCancelled(&WaypointNavigator::onGoalAborted, this);
-    }
+    // if (!succeddedNav2ZClientConnection_.connected())
+    // {
+    //   this->succeddedNav2ZClientConnection_ =
+    //     client_->onSucceeded(&WaypointNavigator::onGoalReached, this);
+    //   this->cancelledNav2ZClientConnection_ =
+    //     client_->onAborted(&WaypointNavigator::onGoalCancelled, this);
+    //   this->abortedNav2ZClientConnection_ =
+    //     client_->onCancelled(&WaypointNavigator::onGoalAborted, this);
+    // }
+
+    auto callbackptr = resultCallback.lock();
+    succeddedNav2ZClientConnection_ = this->getStateMachine()->createSignalConnection(
+      *callbackptr, &WaypointNavigator::onNavigationResult, this);
 
     return client_->sendGoal(goal, resultCallback);
+    //return client_->sendGoal(goal);
   }
   else
   {
@@ -252,6 +257,26 @@ WaypointNavigator::sendNextGoal(
   return std::nullopt;
 }
 
+void WaypointNavigator::onNavigationResult(const ClNav2Z::WrappedResult & r)
+{
+  if (r.code == rclcpp_action::ResultCode::SUCCEEDED)
+  {
+    this->onGoalReached(r);
+  }
+  else if (r.code == rclcpp_action::ResultCode::ABORTED)
+  {
+    this->onGoalAborted(r);
+  }
+  else if (r.code == rclcpp_action::ResultCode::CANCELED)
+  {
+    this->onGoalCancelled(r);
+  }
+  else
+  {
+    this->onGoalAborted(r);
+  }
+}
+
 void WaypointNavigator::insertWaypoint(int index, geometry_msgs::msg::Pose & newpose)
 {
   if (index >= 0 && index <= (int)waypoints_.size())

smacc2_dev_tools/.vscode/settings.json

@@ -118,6 +118,6 @@
     "python.analysis.extraPaths": [
         "/opt/ros/galactic/lib/python3.8/site-packages"
     ],
-    "cmake.sourceDirectory": "${workspaceFolder}/smacc2",
+    "cmake.sourceDirectory": "/home/geus/Desktop/smacc2_ws/src/SMACC2/.work/target_ws/build/smacc2_msgs/smacc2_msgs__py",
 
 }