From 529ba1f20c7c38e662673ebe37920abd0a8a27c6 Mon Sep 17 00:00:00 2001 From: Nicolas Schodet Date: Fri, 10 May 2013 07:38:26 +0200 Subject: digital/io-hub/src/apbirthday: add debug draw --- digital/io-hub/src/common-cc/simu_report.host.cc | 48 +++++++++++++++++++++++- digital/io-hub/src/common-cc/simu_report.host.hh | 11 ++++++ host/simu/robots/apbirthday/model/bag.py | 1 + host/simu/robots/apbirthday/view/bag.py | 2 + 4 files changed, 61 insertions(+), 1 deletion(-) 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) -- cgit v1.2.3