summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorNicolas Schodet2013-05-10 07:38:26 +0200
committerNicolas Schodet2013-05-10 09:19:58 +0200
commit529ba1f20c7c38e662673ebe37920abd0a8a27c6 (patch)
tree8a973e9c9b17e03937b78ec11da10e5e3aa735b6
parent0eefadebd31eba9c115a862790123117066395f9 (diff)
digital/io-hub/src/apbirthday: add debug draw
-rw-r--r--digital/io-hub/src/common-cc/simu_report.host.cc48
-rw-r--r--digital/io-hub/src/common-cc/simu_report.host.hh11
-rw-r--r--host/simu/robots/apbirthday/model/bag.py1
-rw-r--r--host/simu/robots/apbirthday/view/bag.py2
4 files changed, 61 insertions, 1 deletions
diff --git a/digital/io-hub/src/common-cc/simu_report.host.cc b/digital/io-hub/src/common-cc/simu_report.host.cc
index e28df533..d050fa3b 100644
--- a/digital/io-hub/src/common-cc/simu_report.host.cc
+++ b/digital/io-hub/src/common-cc/simu_report.host.cc
@@ -24,11 +24,17 @@
#include "simu_report.host.hh"
SimuReport::SimuReport (ucoo::Host &host)
- : node_ (host.get_node ())
+ : node_ (host.get_node ()), draw_msg_ (0)
{
std::string instance (host.get_instance ());
pos_mtype_ = node_.reserve (instance + ":pos-report");
path_mtype_ = node_.reserve (instance + ":path");
+ draw_mtype_ = node_.reserve (instance + ":debug-draw");
+}
+
+SimuReport::~SimuReport ()
+{
+ delete draw_msg_;
}
void
@@ -50,3 +56,43 @@ SimuReport::path (const vect_t *pos, int pos_nb)
node_.send (msg);
}
+void
+SimuReport::draw_start ()
+{
+ ucoo::assert (!draw_msg_);
+ draw_msg_ = new ucoo::mex::Msg (draw_mtype_);
+}
+
+void
+SimuReport::draw_send ()
+{
+ node_.send (*draw_msg_);
+ delete draw_msg_;
+ draw_msg_ = 0;
+}
+
+void
+SimuReport::draw_circle (const vect_t &p, int radius, uint8_t color)
+{
+ draw_msg_->push ("BhhhB") << 'c' << p.x << p.y << radius << color;
+}
+
+void
+SimuReport::draw_segment (const vect_t &p1, const vect_t &p2, uint8_t color)
+{
+ draw_msg_->push ("BhhhhB") << 's' << p1.x << p1.y << p2.x << p2.y <<
+ color;
+}
+
+void
+SimuReport::draw_point (const vect_t &p, uint8_t color)
+{
+ draw_msg_->push ("BhhB") << 'p' << p.x << p.y << color;
+}
+
+void
+SimuReport::draw_number (const vect_t &p, int number)
+{
+ draw_msg_->push ("Bhhl") << 'n' << p.x << p.y << number;
+}
+
diff --git a/digital/io-hub/src/common-cc/simu_report.host.hh b/digital/io-hub/src/common-cc/simu_report.host.hh
index eaf50281..8a9481cd 100644
--- a/digital/io-hub/src/common-cc/simu_report.host.hh
+++ b/digital/io-hub/src/common-cc/simu_report.host.hh
@@ -32,14 +32,25 @@ class SimuReport
public:
/// Constructor.
SimuReport (ucoo::Host &host);
+ /// Destructor.
+ ~SimuReport ();
/// Report positions.
void pos (vect_t *pos, int pos_nb, uint8_t id);
/// Report path.
void path (const vect_t *pos, int pos_nb);
+ /// Debug draw.
+ void draw_start ();
+ void draw_send ();
+ void draw_circle (const vect_t &p, int radius, uint8_t color);
+ void draw_segment (const vect_t &p1, const vect_t &p2, uint8_t color);
+ void draw_point (const vect_t &p, uint8_t color);
+ void draw_number (const vect_t &p, int number);
private:
ucoo::mex::Node &node_;
ucoo::mex::mtype_t pos_mtype_;
ucoo::mex::mtype_t path_mtype_;
+ ucoo::mex::mtype_t draw_mtype_;
+ ucoo::mex::Msg *draw_msg_;
};
#endif // simu_report_host_hh
diff --git a/host/simu/robots/apbirthday/model/bag.py b/host/simu/robots/apbirthday/model/bag.py
index 0eb995f4..5cc35c6b 100644
--- a/host/simu/robots/apbirthday/model/bag.py
+++ b/host/simu/robots/apbirthday/model/bag.py
@@ -97,4 +97,5 @@ class Bag:
scheduler, 0., 1., .1, .1, 0.)
self.path = link_bag.io_hub.path
self.pos_report = link_bag.io_hub.pos_report
+ self.debug_draw = link_bag.io_hub.debug_draw
diff --git a/host/simu/robots/apbirthday/view/bag.py b/host/simu/robots/apbirthday/view/bag.py
index 57c83049..e545522d 100644
--- a/host/simu/robots/apbirthday/view/bag.py
+++ b/host/simu/robots/apbirthday/view/bag.py
@@ -27,6 +27,7 @@ from simu.view.distance_sensor import DistanceSensor
from simu.view.distance_sensor_us import DistanceSensorUS
from simu.view.path import Path
from simu.view.pos_report import PosReport
+from simu.view.debug_draw import DebugDraw
from simu.robots.apbirthday.view.robot import Robot
class Bag:
@@ -47,4 +48,5 @@ class Bag:
self.cake_back = DistanceSensor (self.robot, model_bag.cake_back)
self.path = Path (table, model_bag.path)
self.pos_report = PosReport (table, model_bag.pos_report)
+ self.debug_draw = DebugDraw (table, model_bag.debug_draw)