[bugfix] fix crash on reach action goal received

Author: alaurenzi

src/problem/impl/Cartesian.cpp

@@ -405,6 +405,13 @@ bool CartesianTaskImpl::setWayPoints(const Trajectory::WayPointVector & way_poin
         return false;
     }
 
+    if(getActivationState() == ActivationState::Disabled)
+    {
+        XBot::Logger::error("Unable to set target pose. Task '%s' is in DISABLED mode \n",
+                            getName().c_str());
+        return false;
+    }
+
     _state = State::Reaching;
     _trajectory->clear();
     _trajectory->addWayPoint(getTime(), _T);

src/ros/server_api/CartesianRos.cpp

@@ -303,7 +303,8 @@ ReachActionManager::ReachActionManager(rclcpp::Node::SharedPtr node,
                                        CartesianTask::Ptr task):
     _task(task),
     _state(ReachActionState::IDLE),
-    _name(task_name)
+    _name(task_name),
+    _node(node)
 {
     using namespace std::placeholders;