order_daemon.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2022 Technical University of Munich, Chair of Materials Handling,
3  * Material Flow, Logistics – All Rights Reserved
4  *
5  * You may use, distribute and modify this code under the terms of the 3-clause
6  * BSD license. You should have received a copy of that license with this file.
7  * If not, please write to {kontakt.fml@ed.tum.de}.
8  */
9 
10 #include "vda5050_connector/order_daemon.h"
11 #include <iostream>
12 #include <vector>
13 #include <string>
14 #include "std_msgs/String.h"
15 #include "std_msgs/Bool.h"
16 #include "std_msgs/Int32.h"
17 #include "vda5050_msgs/Order.h"
18 #include "vda5050_msgs/OrderActions.h"
19 #include "vda5050_msgs/Action.h"
20 #include "vda5050_msgs/ActionState.h"
21 #include "vda5050_msgs/AGVPosition.h"
22 #include "vda5050_msgs/OrderMotion.h"
23 #include "vda5050_msgs/NodeState.h"
24 #include "vda5050_msgs/EdgeState.h"
25 
26 using namespace std;
27 
28 /*-------------------------------------CurrentOrder--------------------------------------------*/
29 
30 CurrentOrder::CurrentOrder(const vda5050_msgs::Order::ConstPtr& incomingOrder)
31 {
32  actionsFinished = false;
33  actionCancellationComplete = false;
34 
35  orderId = incomingOrder->orderId;
36  orderUpdateId = incomingOrder->orderUpdateId;
37  zoneSetId = incomingOrder->zoneSetId;
38  edgeStates = deque<vda5050_msgs::Edge>({incomingOrder->edges.begin(), incomingOrder->edges.end()});
39  nodeStates = deque<vda5050_msgs::Node>({incomingOrder->nodes.begin(), incomingOrder->nodes.end()});
40 }
41 
42 bool CurrentOrder::compareOrderId(string orderIdToCompare)
43 {
44  return this->orderId == orderIdToCompare;
45 }
46 
47 string CurrentOrder::compareOrderUpdateId(int orderUpdateIdToCompare)
48 {
49  if (orderUpdateIdToCompare == this->orderUpdateId)
50  return "EQUAL";
51  else if (orderUpdateIdToCompare > this->orderUpdateId)
52  return "HIGHER";
53  else if (orderUpdateIdToCompare < this->orderUpdateId)
54  return "LOWER";
55  else
56  return "ERROR";
57 }
58 
59 bool CurrentOrder::compareBase(string startOfNewBaseNodeId, int startOfNewBaseSequenceId)
60 {
61  return (this->nodeStates.back().nodeId == startOfNewBaseNodeId) &&
62  (this->nodeStates.back().sequenceId == startOfNewBaseSequenceId);
63 }
64 
65 string CurrentOrder::getOrderId()
66 {
67  return this->orderId;
68 }
69 
70 int CurrentOrder::getOrderUpdateId()
71 {
72  return this->orderUpdateId;
73 }
74 
75 void CurrentOrder::setOrderUpdateId(int incomingUpdateId)
76 {
77  this->orderUpdateId = incomingUpdateId;
78 }
79 
80 bool CurrentOrder::isActive()
81 {
82  if (!this->nodeStates.empty())
83  {
84  if (!this->edgeStates.empty())
85  {
86  if (this->actionStates.empty())
87  return true;
88  }
89  }
90  return false;
91 }
92 
93 string CurrentOrder::findNodeEdge(int currSequenceId)
94 {
95  if (edgeStates.front().sequenceId == currSequenceId)
96  return "EDGE";
97  else if (nodeStates.front().sequenceId == currSequenceId)
98  return "NODE";
99  else
100  return "SEQUENCE ERROR";
101 }
102 
103 vda5050_msgs::Node CurrentOrder::getLastNodeInBase()
104 {
106  auto it = find_if(nodeStates.begin(), nodeStates.end(), [] (const vda5050_msgs::Node &node) {return node.released == false;});
108  if (it != nodeStates.end())
109  return *(it--);
111  else
112  return nodeStates.back();
113 }
114 
115 void CurrentOrder::sendActions(ros::Publisher actionPublisher)
116 {
117  int maxSequenceId = max(edgeStates.back().sequenceId, nodeStates.back().sequenceId);
118  deque<vda5050_msgs::Edge>::iterator edgeIt = edgeStates.begin();
119  deque<vda5050_msgs::Node>::iterator nodeIt = nodeStates.begin();
120 
122  if (nodeStates.front().actions.empty())
123  this->actionsFinished = true;
124 
125  for (int currSeq = 0; currSeq <= maxSequenceId; currSeq++)
126  {
127  if ((edgeIt->sequenceId == currSeq) && (edgeIt->released) && !(edgeIt->actions.empty()))
128  {
129  vda5050_msgs::OrderActions msg;
130  msg.orderActions = edgeIt->actions;
131  msg.orderId = orderId;
132  actionPublisher.publish(msg);
133  *edgeIt++;
134  }
135  if ((nodeIt->sequenceId == currSeq) && (nodeIt->released) && !(nodeIt->actions.empty()))
136  {
137  vda5050_msgs::OrderActions msg;
138  msg.orderActions = nodeIt->actions;
139  msg.orderId = orderId;
140  actionPublisher.publish(msg);
141  *nodeIt++;
142  }
143  }
144 }
145 
146 void CurrentOrder::sendNodeStates(ros::Publisher nodeStatesPublisher)
147 {
148  for (auto const &state_it : this->nodeStates)
149  {
151  vda5050_msgs::NodeState state_msg;
152  state_msg.nodeId = state_it.nodeId;
153  state_msg.sequenceId = state_it.sequenceId;
154  state_msg.nodeDescription = state_it.nodeDescription;
155  state_msg.position = state_it.nodePosition;
156  state_msg.released = state_it.released;
157 
159  nodeStatesPublisher.publish(state_msg);
160 
161  ROS_INFO("x: %f, y: %f", state_it.nodePosition.x, state_it.nodePosition.y);
162  }
163 }
164 
165 void CurrentOrder::sendEdgeStates(ros::Publisher edgeStatesPublisher)
166 {
167  for (auto const &state_it : this->edgeStates)
168  {
170  vda5050_msgs::EdgeState state_msg;
171  state_msg.edgeId = state_it.edgeId;
172  state_msg.sequenceId = state_it.sequenceId;
173  state_msg.edgeDescription = state_it.edgeDescription;
174  state_msg.trajectory = state_it.trajectory;
175  state_msg.released = state_it.released;
176 
178  edgeStatesPublisher.publish(state_msg);
179  }
180 }
181 
182 
183 /*-------------------------------------AGVPosition--------------------------------------------*/
184 
185 AGVPosition::AGVPosition()
186 {
187  x = 0;
188  y = 0;
189  theta = 0;
190  mapId = "initializing...";
191 }
192 
193 void AGVPosition::updatePosition(float new_x, float new_y, float new_theta, string new_mapId)
194 {
195  x = new_x;
196  y = new_y;
197  theta = new_theta;
198  mapId = new_mapId;
199 }
200 
201 float AGVPosition::nodeDistance(float node_x, float node_y)
202 {
203  return sqrt(pow(node_x-x, 2)+pow(node_y-y,2));
204 }
205 
206 float AGVPosition::getTheta()
207 {
208  return theta;
209 }
210 
211 /*-------------------------------------OrderDaemon--------------------------------------------*/
212 
213 OrderDaemon::OrderDaemon() : Daemon(&(this->nh), "order_daemon")
214 {
216  LinkPublishTopics(&(this->nh));
217  LinkSubscriptionTopics(&(this->nh));
218 
220  orderCancelSub = nh.subscribe("orderCancelRequest", 1000, &OrderDaemon::OrderCancelRequestCallback, this);
221  agvPositionSub = nh.subscribe("agvPosition", 1000, &OrderDaemon::AgvPositionCallback, this);
222  allActionsCancelledSub = nh.subscribe("allActionsCancelled", 1000, &OrderDaemon::allActionsCancelledCallback, this);
223  orderActionPub = nh.advertise<vda5050_msgs::OrderActions>("orderAction", 1000);
224  orderCancelPub = nh.advertise<std_msgs::String>("orderCancelResponse", 1000);
225  orderTriggerPub = nh.advertise<std_msgs::String>("orderTrigger", 1000);
226  nodeStatesPub = nh.advertise<vda5050_msgs::NodeState>("nodeStates", 1000);
227  edgeStatesPub = nh.advertise<vda5050_msgs::EdgeState>("edgeStates", 1000);
228  lastNodeIdPub = nh.advertise<std_msgs::String>("lastNodeId", 1000);
229  lastNodeSequenceIdPub = nh.advertise<std_msgs::Int32>("lastNodeSequenceId", 1000);
230  orderIdPub = nh.advertise<std_msgs::String>("orderId", 1000);
231  orderUpdateIdPub = nh.advertise<std_msgs::Int32>("orderUpdateId", 1000);
232 }
233 
234 void OrderDaemon::LinkPublishTopics(ros::NodeHandle *nh)
235 {
236  map<string,string>topicList = GetTopicPublisherList();
237  std::string topic_index;
238 
239  for(const auto& elem : topicList)
240  {
241  topic_index = GetTopic(elem.first);
242  ROS_INFO("topic_index = %s",topic_index.c_str());
243  if (CheckTopic(elem.first,"orderMotion"))
244  messagePublisher[topic_index] = nh->advertise<vda5050_msgs::OrderMotion>(elem.second,1000);
245  if (CheckTopic(elem.first,"prDriving")) {
246  messagePublisher[topic_index] = nh->advertise<std_msgs::String>(elem.second,1000);
247  }
248  }
249 }
250 
251 void OrderDaemon::LinkSubscriptionTopics(ros::NodeHandle *nh)
252 {
253  map<string,string>topicList = GetTopicSubscriberList();
254  for(const auto& elem : topicList)
255  {
256  if (CheckTopic(elem.first,"orderFromMc"))
257  subscribers[elem.first] = nh->subscribe(elem.second,1000,&OrderDaemon::OrderCallback, this);
258  if (CheckTopic(elem.first,"actionStates"))
259  subscribers[elem.first] = nh->subscribe(elem.second,1000,&OrderDaemon::ActionStateCallback, this);
260  if (CheckTopic(elem.first, "driving"))
261  subscribers[elem.first] = nh->subscribe(elem.second, 1000, &OrderDaemon::DrivingCallback, this);
262  }
263 }
264 
265 bool OrderDaemon::validationCheck(const vda5050_msgs::Order::ConstPtr& msg)
266 {
270  return true;
271 }
272 
273 bool OrderDaemon::inDevRange(vda5050_msgs::Node node)
274 {
275  return ((agvPosition.nodeDistance(node.nodePosition.x,
276  node.nodePosition.y)) <=
277  node.nodePosition.allowedDeviationXY) &&
278  (agvPosition.getTheta() <= node.nodePosition.allowedDeviationTheta);
279 }
280 
281 void OrderDaemon::triggerNewActions(string nodeOrEdge)
282 {
283  if (nodeOrEdge == "NODE")
284  {
285  if (this->currentOrders.front().nodeStates.front().released)
286  {
287  if (!this->currentOrders.front().nodeStates.front().actions.empty())
288  {
289  for (auto const &action : this->currentOrders.front().nodeStates.front().actions)
290  {
291  std_msgs::String msg;
292  msg.data = action.actionId;
293  orderTriggerPub.publish(msg);
294  }
295  }
296  else
297  currentOrders.front().actionsFinished = true;
298  }
299  }
300  else if (nodeOrEdge == "EDGE")
301  {
302  if (this->currentOrders.front().edgeStates.front().released)
303  {
304  if (!this->currentOrders.front().edgeStates.front().actions.empty())
305  {
306  for (auto const &action : this->currentOrders.front().edgeStates.front().actions)
307  {
308  std_msgs::String msg;
309  msg.data = action.actionId;
310  orderTriggerPub.publish(msg);
311  }
312  }
313  else
314  currentOrders.front().actionsFinished = true;
315  }
316  }
317  else
318  ROS_ERROR("Neither node nor edge matching sequence ID!");
319 }
320 
321 void OrderDaemon::sendMotionCommand()
322 {
323  vda5050_msgs::Edge edge = currentOrders.front().edgeStates.front();
324  vda5050_msgs::OrderMotion msg;
325  if (edge.released)
326  {
328  msg.maxSpeed = edge.maxSpeed;
329  msg.maxRotationSpeed = edge.maxRotationSpeed;
330  msg.maxHeight = edge.maxHeight;
331  msg.minHeight = edge.minHeight;
332  msg.direction = edge.direction;
333  msg.rotationAllowed = edge.rotationAllowed;
334  msg.orientation = edge.orientation;
335  msg.length = edge.length;
336 
338  if (edge.trajectory.knotVector.empty())
339  msg.target = currentOrders.front().nodeStates.front().nodePosition;
340  else
341  msg.trajectory = edge.trajectory;
342  messagePublisher["orderMotion"].publish(msg);
343  }
344  else
345  ROS_ERROR("Neither node nor edge matching sequence ID!");
346 }
347 
348 void OrderDaemon::OrderCallback(const vda5050_msgs::Order::ConstPtr &msg)
349 {
350  if (validationCheck(msg))
351  {
352  if (!currentOrders.empty())
353  {
354  if (currentOrders.back().compareOrderId(msg->orderId))
355  {
356  if (currentOrders.back().compareOrderUpdateId(msg->orderUpdateId) == "LOWER")
357  {
358  orderUpdateError(msg->orderId, msg->orderUpdateId);
359  }
360  else if (currentOrders.back().compareOrderUpdateId(msg->orderUpdateId) == "EQUAL")
361  {
362  ROS_WARN_STREAM("Order discarded. Strucure invalid! " << msg->orderId << ", " << msg->orderUpdateId);
363  }
364  else
365  {
366  if (currentOrders.back().isActive())
367  {
368  if (currentOrders.back().compareBase(msg->nodes.front().nodeId,
369  msg->nodes.front().sequenceId))
370  {
371  updateExistingOrder(msg);
372  }
373  else
374  {
375  orderUpdateError(msg->orderId, msg->orderUpdateId);
376  }
377  }
378  else
379  {
380  if (currentOrders.back().compareBase(msg->nodes.front().nodeId,
381  msg->nodes.front().sequenceId))
382  {
383  updateExistingOrder(msg);
384  }
385  else
386  {
387  orderUpdateError(msg->orderId, msg->orderUpdateId);
388  }
389  }
390  }
391  }
392  else
393  {
394  if (currentOrders.back().compareBase(msg->nodes.front().nodeId,
395  msg->nodes.front().sequenceId))
396  {
397  appendNewOrder(msg);
398  }
399  else
400  {
401  orderUpdateError(msg->orderId, msg->orderUpdateId);
402  }
403  }
404  }
405  else
406  {
407  if (inDevRange(msg.get()->nodes.front()))
408  startNewOrder(msg);
409  else
410  orderUpdateError(msg->orderId, msg->orderUpdateId);
411  }
412 
413  }
414  else
415  {
416  orderValidationError(msg->orderId, msg->orderUpdateId);
417  }
418 }
419 
420 void OrderDaemon::OrderCancelRequestCallback(const std_msgs::String::ConstPtr &msg)
421 {
422  ROS_INFO("Received cancel request for order: %s", msg.get()->data.c_str());
423  auto orderToCancel = find_if(currentOrders.begin(), currentOrders.end(),
424  [&msg](CurrentOrder order)
425  { return order.compareOrderId(msg.get()->data); });
426  if (orderToCancel != currentOrders.end())
427  {
428  if (isDriving == true)
429  {
430  std_msgs::String msg;
431  msg.data = "PAUSE";
432  messagePublisher["prDriving"].publish(msg);
433  }
434  ordersToCancel.push_back(msg.get()->data);
435  }
436  else
437  ROS_ERROR("Order to cancel not found: %s", msg.get()->data.c_str());
438 }
439 
440 void OrderDaemon::allActionsCancelledCallback(const std_msgs::String::ConstPtr &msg)
441 {
442  auto orderToCancel = find_if(currentOrders.begin(), currentOrders.end(),
443  [&msg](CurrentOrder order)
444  { return order.compareOrderId(msg.get()->data); });
445  if (orderToCancel != currentOrders.end())
446  {
447  orderToCancel->actionCancellationComplete = true;
448  }
449 }
450 
451 void OrderDaemon::ActionStateCallback(const vda5050_msgs::ActionState::ConstPtr &msg)
452 {
453  for (auto &order : currentOrders)
454  {
455  auto it = find(order.actionStates.begin(), order.actionStates.end(), msg.get()->actionID);
456  if (it != order.actionStates.end())
457  {
458  if (msg.get()->actionStatus == "FINISHED")
459  {
460  order.actionStates.erase(it);
461  if (order.actionStates.empty())
462  {
463  order.actionsFinished = true;
464  }
465  }
466  else if (msg.get()->actionStatus == "FAILED");
468  }
469  }
470 }
471 
472 void OrderDaemon::AgvPositionCallback(const vda5050_msgs::AGVPosition::ConstPtr &msg)
473 {
474  agvPosition.updatePosition(msg->x, msg->y, msg->theta, msg->mapId);
475  ROS_INFO("Got new position: %f, %f", msg->x, msg->y);
476  if (!currentOrders.empty() && ordersToCancel.empty())
477  {
478  if (currentOrders.front().findNodeEdge(currSequenceId) == "EDGE")
479  {
480  if (inDevRange(currentOrders.front().nodeStates.front()))
481  {
482  if (currentOrders.front().actionsFinished)
483  {
485  currentOrders.front().actionsFinished = false;
486 
488  currentOrders.front().edgeStates.pop_front();
489 
491  if (!(currentOrders.front().nodeStates.empty()))
492  {
493  currSequenceId = currentOrders.front().nodeStates.front().sequenceId;
494  triggerNewActions("NODE");
495  for (auto const &action : currentOrders.front().nodeStates.front().actions)
496  currentOrders.front().actionStates.push_back(action.actionId);
497  }
499  else
500  ROS_ERROR("Missing node in current order!");
502  }
503  }
504  }
505  else if (currentOrders.front().findNodeEdge(currSequenceId) == "NODE")
506  {
507  if (currentOrders.front().actionsFinished)
508  {
509  ROS_INFO("Node passed through!");
511  currentOrders.front().actionsFinished = false;
512 
514  std_msgs::String lastNodeIdMsg;
515  lastNodeIdMsg.data = currentOrders.front().nodeStates.front().nodeId;
516  lastNodeIdPub.publish(lastNodeIdMsg);
517 
519  std_msgs::Int32 lastNodeSequenceIdMsg;
520  lastNodeSequenceIdMsg.data = currentOrders.front().nodeStates.front().sequenceId;
521  lastNodeSequenceIdPub.publish(lastNodeSequenceIdMsg);
522 
524  currentOrders.front().nodeStates.pop_front();
525 
527  if (currentOrders.front().edgeStates.front().released &&
528  !(currentOrders.front().edgeStates.empty()))
529  {
530  currSequenceId = currentOrders.front().edgeStates.front().sequenceId;
531  triggerNewActions("EDGE");
532  sendMotionCommand();
533  for (auto const &action : currentOrders.front().edgeStates.front().actions)
534  currentOrders.front().actionStates.push_back(action.actionId);
535  }
536  else
537  {
538  currentOrders.erase(currentOrders.begin());
539  if (currentOrders.empty())
540  {
542  std_msgs::String orderIdMsg;
543  orderIdMsg.data = currentOrders.front().getOrderId();
544  orderIdPub.publish(orderIdMsg);
545 
547  std_msgs::Int32 orderUpdateIdMsg;
548  orderUpdateIdMsg.data = currentOrders.front().getOrderUpdateId();
549  orderUpdateIdPub.publish(orderUpdateIdMsg);
550  }
551  }
552  }
553  }
554  else
555  ;//ROS_ERROR("Neither node nor edge matching position update!");
556  }
557 }
558 
559 void OrderDaemon::DrivingCallback(const std_msgs::Bool::ConstPtr &msg)
560 {
561  isDriving = msg->data;
562 }
563 
564 void OrderDaemon::startNewOrder(const vda5050_msgs::Order::ConstPtr& msg)
565 {
567  CurrentOrder newOrder(msg);
568  currentOrders.push_back(newOrder);
569 
571  currSequenceId = msg->nodes.front().sequenceId;
572 
574  sendMotionCommand();
575 
577  currentOrders.back().sendActions(orderActionPub);
578 
580  if (!currentOrders.back().nodeStates.front().actions.empty())
581  {
582  triggerNewActions("NODE");
583  for (auto const &action : currentOrders.front().nodeStates.front().actions)
584  currentOrders.front().actionStates.push_back(action.actionId);
585  }
586 
588  currentOrders.front().sendNodeStates(nodeStatesPub);
589  currentOrders.front().sendEdgeStates(edgeStatesPub);
590 
592  std_msgs::String orderIdMsg;
593  orderIdMsg.data = currentOrders.front().getOrderId();
594  orderIdPub.publish(orderIdMsg);
595 
597  std_msgs::Int32 orderUpdateIdMsg;
598  orderUpdateIdMsg.data = currentOrders.front().getOrderUpdateId();
599  orderUpdateIdPub.publish(orderUpdateIdMsg);
600 
601  ROS_INFO("Started new order: %s", msg->orderId.c_str());
602 }
603 
604 void OrderDaemon::appendNewOrder(const vda5050_msgs::Order::ConstPtr& msg)
605 {
607  currentOrders.front().edgeStates.erase(remove_if(currentOrders.front().edgeStates.begin(),
608  currentOrders.front().edgeStates.end(),
609  [](vda5050_msgs::Edge delEdge)
610  { return !delEdge.released; }));
611  currentOrders.front().nodeStates.erase(remove_if(currentOrders.front().nodeStates.begin(),
612  currentOrders.front().nodeStates.end(),
613  [](vda5050_msgs::Node delNode)
614  { return !delNode.released; }));
615 
617  CurrentOrder newOrder(msg);
618  currentOrders.push_back(newOrder);
619  newOrder.sendActions(orderActionPub);
620 
622  currentOrders.front().sendNodeStates(nodeStatesPub);
623  currentOrders.front().sendEdgeStates(edgeStatesPub);
624 }
625 
626 void OrderDaemon::updateExistingOrder(const vda5050_msgs::Order::ConstPtr& msg)
627 {
629  currentOrders.front().edgeStates.erase(remove_if(currentOrders.front().edgeStates.begin(),
630  currentOrders.front().edgeStates.end(),
631  [](vda5050_msgs::Edge delEdge)
632  { return !delEdge.released; }));
633  currentOrders.front().nodeStates.erase(remove_if(currentOrders.front().nodeStates.begin(),
634  currentOrders.front().nodeStates.end(),
635  [](vda5050_msgs::Node delNode)
636  { return !delNode.released; }));
637 
639  for (auto const &newEdge: msg->edges)
640  currentOrders.front().edgeStates.push_back(newEdge);
641  for (auto const &newNode: msg->nodes)
642  currentOrders.front().nodeStates.push_back(newNode);
643 
644  currentOrders.front().setOrderUpdateId(msg.get()->orderUpdateId);
645 
647  CurrentOrder newOrder(msg);
648  newOrder.sendActions(orderActionPub);
649 
651  currentOrders.front().sendNodeStates(nodeStatesPub);
652  currentOrders.front().sendEdgeStates(edgeStatesPub);
653 
655  std_msgs::Int32 orderUpdateMsg;
656  orderUpdateMsg.data = msg.get()->orderUpdateId;
657  orderUpdateIdPub.publish(orderUpdateMsg);
658 }
659 
660 void OrderDaemon::UpdateOrders()
661 {
662  if (!ordersToCancel.empty())
663  {
664  if (!isDriving)
665  {
666  for (vector<string>::iterator orderIdIt = ordersToCancel.begin(); orderIdIt!= ordersToCancel.end();)
667  {
668  auto order = find_if(currentOrders.begin(), currentOrders.end(),
669  [&orderIdIt](CurrentOrder currOrd)
670  { return currOrd.compareOrderId(*orderIdIt); });
671  if (order != currentOrders.end())
672  {
673  if (order->actionCancellationComplete)
674  {
676  currentOrders.erase(std::remove_if(currentOrders.begin(), currentOrders.end(),
677  [&orderIdIt](CurrentOrder &order)
678  { return order.compareOrderId(*orderIdIt); }),
679  currentOrders.end());
681  std_msgs::String msg;
682  msg.data = *orderIdIt;
683  orderCancelPub.publish(msg);
684 
686  ordersToCancel.erase(orderIdIt);
687  }
688  else
689  orderIdIt++;
690  }
691  else
692  {
693  ROS_WARN("Order to cancel not found: %s", (*orderIdIt).c_str());
694  }
695  }
696  }
697  }
698 }
699 
700 void OrderDaemon::orderUpdateError(string orderId, int orderUpdateId)
701 {
702  std_msgs::String rejectMsg;
703  stringstream ss;
704  ss << "orderUpdateError: " << orderId << ", " << orderUpdateId;
705  rejectMsg.data = ss.str();
706  errorPublisher.publish(rejectMsg);
707 }
708 
709 void OrderDaemon::orderValidationError(string orderId, int orderUpdateId)
710 {
711  std_msgs::String rejectMsg;
712  stringstream ss;
713  ss << "orderValidationError: " << orderId << ", " << orderUpdateId;
714  rejectMsg.data = ss.str();
715  errorPublisher.publish(rejectMsg);
716 }
717 
718 int main(int argc, char **argv)
719 {
720  ros::init(argc, argv, "order_daemon");
721 
722  OrderDaemon orderDaemon;
723 
724  while(ros::ok())
725  {
726  orderDaemon.UpdateOrders();
727  ros::spinOnce();
728  }
729  return 0;
730 }
OrderDaemon::ActionStateCallback
void ActionStateCallback(const vda5050_msgs::ActionState::ConstPtr &msg)
Definition: order_daemon.cpp:451
OrderDaemon::isDriving
bool isDriving
Definition: order_daemon.h:286
OrderDaemon::orderIdPub
ros::Publisher orderIdPub
Definition: order_daemon.h:275
CurrentOrder::compareBase
bool compareBase(string startOfNewBaseNodeId, int startOfNewBaseSequenceId)
Definition: order_daemon.cpp:59
Daemon::GetTopicPublisherList
std::map< std::string, std::string > GetTopicPublisherList()
Definition: src/daemon.cpp:46
OrderDaemon::AgvPositionCallback
void AgvPositionCallback(const vda5050_msgs::AGVPosition::ConstPtr &msg)
Definition: order_daemon.cpp:472
OrderDaemon::inDevRange
bool inDevRange(vda5050_msgs::Node node)
Definition: order_daemon.cpp:273
ros::Publisher
OrderDaemon::appendNewOrder
void appendNewOrder(const vda5050_msgs::Order::ConstPtr &msg)
Definition: order_daemon.cpp:604
CurrentOrder::CurrentOrder
CurrentOrder(const vda5050_msgs::Order::ConstPtr &incomingOrder)
Definition: order_daemon.cpp:30
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
CurrentOrder::findNodeEdge
string findNodeEdge(int currSequenceId)
Definition: order_daemon.cpp:93
Daemon::errorPublisher
ros::Publisher errorPublisher
Definition: daemon.h:64
OrderDaemon::currSequenceId
int currSequenceId
Definition: order_daemon.h:289
OrderDaemon::orderActionPub
ros::Publisher orderActionPub
Definition: order_daemon.h:254
OrderDaemon::ordersToCancel
vector< string > ordersToCancel
Definition: order_daemon.h:283
OrderDaemon::orderCancelPub
ros::Publisher orderCancelPub
Definition: order_daemon.h:257
OrderDaemon::agvPositionSub
ros::Subscriber agvPositionSub
Definition: order_daemon.h:246
CurrentOrder::isActive
bool isActive()
Definition: order_daemon.cpp:80
Daemon::subscribers
std::map< std::string, ros::Subscriber > subscribers
Definition: daemon.h:59
OrderDaemon::orderUpdateError
void orderUpdateError(string orderId, int orderUpdateId)
Definition: order_daemon.cpp:700
ros::spinOnce
ROSCPP_DECL void spinOnce()
AGVPosition::updatePosition
void updatePosition(float new_x, float new_y, float new_theta, string new_mapId)
Definition: order_daemon.cpp:193
OrderDaemon::currentOrders
vector< CurrentOrder > currentOrders
Definition: order_daemon.h:232
OrderDaemon::orderUpdateIdPub
ros::Publisher orderUpdateIdPub
Definition: order_daemon.h:278
main
int main(int argc, char **argv)
Definition: order_daemon.cpp:718
ros::Publisher::publish
void publish(const boost::shared_ptr< M > &message) const
OrderDaemon::agvPosition
AGVPosition agvPosition
Definition: order_daemon.h:235
ros::NodeHandle::advertise
Publisher advertise(AdvertiseOptions &ops)
OrderDaemon::triggerNewActions
void triggerNewActions(string nodeOrEdge)
Definition: order_daemon.cpp:281
ros::ok
ROSCPP_DECL bool ok()
OrderDaemon::edgeStatesPub
ros::Publisher edgeStatesPub
Definition: order_daemon.h:266
CurrentOrder::sendEdgeStates
void sendEdgeStates(ros::Publisher edgeStatesPublisher)
Definition: order_daemon.cpp:165
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
Daemon::GetTopicSubscriberList
std::map< std::string, std::string > GetTopicSubscriberList()
Definition: src/daemon.cpp:51
CurrentOrder::setOrderUpdateId
void setOrderUpdateId(int incomingUpdateId)
Definition: order_daemon.cpp:75
OrderDaemon::sendMotionCommand
void sendMotionCommand()
Definition: order_daemon.cpp:321
OrderDaemon::nodeStatesPub
ros::Publisher nodeStatesPub
Definition: order_daemon.h:263
orderToCancel
Definition: action_daemon.h:118
Daemon::GetTopic
std::string GetTopic(std::string hierarchical_topic)
Definition: src/daemon.cpp:128
CurrentOrder::sendNodeStates
void sendNodeStates(ros::Publisher nodeStatesPublisher)
Definition: order_daemon.cpp:146
OrderDaemon::DrivingCallback
void DrivingCallback(const std_msgs::Bool::ConstPtr &msg)
Definition: order_daemon.cpp:559
OrderDaemon
Definition: order_daemon.h:229
Daemon::messagePublisher
std::map< std::string, ros::Publisher > messagePublisher
Definition: daemon.h:54
OrderDaemon::startNewOrder
void startNewOrder(const vda5050_msgs::Order::ConstPtr &msg)
Definition: order_daemon.cpp:564
ROS_WARN
#define ROS_WARN(...)
CurrentOrder::getOrderUpdateId
int getOrderUpdateId()
Definition: order_daemon.cpp:70
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
OrderDaemon::UpdateOrders
void UpdateOrders()
Definition: order_daemon.cpp:660
Daemon::nh
ros::NodeHandle nh
Definition: daemon.h:67
Daemon
Definition: daemon.h:29
OrderDaemon::updateExistingOrder
void updateExistingOrder(const vda5050_msgs::Order::ConstPtr &msg)
Definition: order_daemon.cpp:626
OrderDaemon::orderValidationError
void orderValidationError(string orderId, int orderUpdateId)
Definition: order_daemon.cpp:709
OrderDaemon::allActionsCancelledCallback
void allActionsCancelledCallback(const std_msgs::String::ConstPtr &msg)
Definition: order_daemon.cpp:440
CurrentOrder::compareOrderUpdateId
string compareOrderUpdateId(int orderUpdateIdToCompare)
Definition: order_daemon.cpp:47
OrderDaemon::orderCancelSub
ros::Subscriber orderCancelSub
Definition: order_daemon.h:243
OrderDaemon::allActionsCancelledSub
ros::Subscriber allActionsCancelledSub
Definition: order_daemon.h:249
OrderDaemon::LinkSubscriptionTopics
void LinkSubscriptionTopics(ros::NodeHandle *nh)
Definition: order_daemon.cpp:251
OrderDaemon::OrderCancelRequestCallback
void OrderCancelRequestCallback(const std_msgs::String::ConstPtr &msg)
Definition: order_daemon.cpp:420
std
ROS_ERROR
#define ROS_ERROR(...)
CurrentOrder::compareOrderId
bool compareOrderId(string orderIdToCompare)
Definition: order_daemon.cpp:42
OrderDaemon::OrderDaemon
OrderDaemon()
Definition: order_daemon.cpp:213
OrderDaemon::LinkPublishTopics
void LinkPublishTopics(ros::NodeHandle *nh)
Definition: order_daemon.cpp:234
AGVPosition::getTheta
float getTheta()
Definition: order_daemon.cpp:206
AGVPosition::AGVPosition
AGVPosition()
Definition: order_daemon.cpp:185
CurrentOrder::getLastNodeInBase
vda5050_msgs::Node getLastNodeInBase()
Definition: order_daemon.cpp:103
CurrentOrder
Definition: order_daemon.h:31
ROS_INFO
#define ROS_INFO(...)
OrderDaemon::OrderCallback
void OrderCallback(const vda5050_msgs::Order::ConstPtr &msg)
Definition: order_daemon.cpp:348
OrderDaemon::lastNodeSequenceIdPub
ros::Publisher lastNodeSequenceIdPub
Definition: order_daemon.h:272
OrderDaemon::validationCheck
bool validationCheck(const vda5050_msgs::Order::ConstPtr &msg)
Definition: order_daemon.cpp:265
OrderDaemon::orderTriggerPub
ros::Publisher orderTriggerPub
Definition: order_daemon.h:260
order_daemon.h
CurrentOrder::sendActions
void sendActions(ros::Publisher actionPublisher)
Definition: order_daemon.cpp:115
OrderDaemon::lastNodeIdPub
ros::Publisher lastNodeIdPub
Definition: order_daemon.h:269
CurrentOrder::getOrderId
string getOrderId()
Definition: order_daemon.cpp:65
ros::NodeHandle
AGVPosition::nodeDistance
float nodeDistance(float node_x, float node_y)
Definition: order_daemon.cpp:201
Daemon::CheckTopic
bool CheckTopic(std::string str1, std::string str2)
Definition: src/daemon.cpp:117

vda5050_connector
Author(s): Florian Rothmeyer , Florian Spiegel
autogenerated on Wed Mar 22 2023 02:38:56