diff --git a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp index 67539f596..9e8f9f563 100644 --- a/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp +++ b/rmf_fleet_adapter/src/rmf_fleet_adapter/TaskManager.cpp @@ -155,10 +155,12 @@ void TaskManager::set_queue( if (i != 0) start = assignments[i-1].state().location(); start.time(a.deployment_time()); + rmf_task_msgs::msg::TaskType task_type_msg; if (const auto request = std::dynamic_pointer_cast(a.request())) { + task_type_msg.type = task_type_msg.TYPE_CLEAN; auto task = rmf_fleet_adapter::tasks::make_clean( request, _context, @@ -169,19 +171,13 @@ void TaskManager::set_queue( std::lock_guard guard(_mutex); _queue.push_back(task); - - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); - msg.state = msg.STATE_QUEUED; - msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); } else if (const auto request = std::dynamic_pointer_cast( a.request())) { + task_type_msg.type = task_type_msg.TYPE_CHARGE_BATTERY; const auto task = tasks::make_charge_battery( request, _context, @@ -192,20 +188,13 @@ void TaskManager::set_queue( std::lock_guard guard(_mutex); _queue.push_back(task); - - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); - msg.state = msg.STATE_QUEUED; - msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); - } else if (const auto request = std::dynamic_pointer_cast( a.request())) { + task_type_msg.type = task_type_msg.TYPE_DELIVERY; const auto task = tasks::make_delivery( request, _context, @@ -214,19 +203,12 @@ void TaskManager::set_queue( a.state()); _queue.push_back(task); - - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); - msg.state = msg.STATE_QUEUED; - msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); - } else if (const auto request = std::dynamic_pointer_cast(a.request())) { + task_type_msg.type = task_type_msg.TYPE_LOOP; const auto task = tasks::make_loop( request, _context, @@ -235,19 +217,25 @@ void TaskManager::set_queue( a.state()); _queue.push_back(task); - - rmf_task_msgs::msg::TaskSummary msg; - msg.task_id = _queue.back()->id(); - msg.task_profile.task_id = _queue.back()->id(); - msg.state = msg.STATE_QUEUED; - msg.robot_name = _context->name(); - this->_context->node()->task_summary()->publish(msg); } else { continue; } + + // publish queued task + rmf_task_msgs::msg::TaskSummary msg; + msg.task_id = _queue.back()->id(); + msg.task_profile.task_id = _queue.back()->id(); + msg.state = msg.STATE_QUEUED; + msg.robot_name = _context->name(); + msg.task_profile.task_type = task_type_msg; + msg.start_time = rmf_traffic_ros2::convert( + _queue.back()->deployment_time()); + msg.start_time = rmf_traffic_ros2::convert( + _queue.back()->finish_state().finish_time()); + this->_context->node()->task_summary()->publish(msg); } } @@ -261,11 +249,8 @@ const std::vector TaskManager::requests() const if (std::dynamic_pointer_cast( task->request())) continue; - requests.push_back(task->request()); - } - return requests; } @@ -315,6 +300,10 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; msg.robot_name = _context->name(); + msg.start_time = rmf_traffic_ros2::convert( + _active_task->deployment_time()); + msg.end_time = rmf_traffic_ros2::convert( + _active_task->finish_state().finish_time()); _context->node()->task_summary()->publish(msg); }, [this, id = _active_task->id()](std::exception_ptr e) @@ -332,6 +321,10 @@ void TaskManager::_begin_next_task() msg.task_id = id; msg.task_profile.task_id = id; msg.robot_name = _context->name(); + msg.start_time = rmf_traffic_ros2::convert( + _active_task->deployment_time()); + msg.end_time = rmf_traffic_ros2::convert( + _active_task->finish_state().finish_time()); _context->node()->task_summary()->publish(msg); // _begin_next_task(); }, @@ -342,6 +335,10 @@ void TaskManager::_begin_next_task() msg.task_profile.task_id = id; msg.state = msg.STATE_COMPLETED; msg.robot_name = _context->name(); + msg.start_time = rmf_traffic_ros2::convert( + _active_task->deployment_time()); + msg.end_time = rmf_traffic_ros2::convert( + _active_task->finish_state().finish_time()); this->_context->node()->task_summary()->publish(msg); _active_task = nullptr;