Fix cancellation, feedback

Signed-off-by: Luca Della Vedova <[email protected]>

Author: luca-della-vedova

nexus_system_orchestrator/src/system_orchestrator.cpp

@@ -860,7 +860,7 @@ void SystemOrchestrator::_halt_job(const std::string& job_id)
         std::make_shared<endpoints::RemovePendingTaskService::ServiceType::Request>();
       req->task_id = task_id;
       const auto resp =
-        this->_workcell_sessions.at(task_id)->remove_pending_task_client->
+        this->_workcell_sessions.at(wc_id)->remove_pending_task_client->
         send_request(req);
       if (!resp->success)
       {

nexus_system_orchestrator/src/workcell_request.cpp

@@ -105,7 +105,9 @@ WorkcellRequest::make_goal()
 void WorkcellRequest::on_feedback(
   endpoints::WorkcellRequestAction::ActionType::Feedback::ConstSharedPtr msg)
 {
-  this->_ctx->task_states.at(this->_task.id) = msg->state;
+  // Will insert a new state if one was not created at work order creation time
+  // i.e. for transportation requests that are done after task assignments
+  this->_ctx->task_states[this->_task.id] = msg->state;
   this->_on_task_progress(this->_ctx->task_states);
 }