add aimpoint

Author: namv1994

docker-compose.yml

@@ -6,4 +6,9 @@ services:    # Must be present exactly once at the beginning of the docker-compo
         ipc: host
         environment:
           - DISPLAY=$DISPLAY
-        command: "/usr/bin/cfsd19-cognition-detectconelane --cid=219  --verbose --freq=10"
+        command: "cfsd-logic-pathplanner --cid=253 --debug --freq=10"
+        
+    cfsd19-logic-aimpoint:
+      image: lowitz/aimpoint:v0.0.2
+      network_mode: "host"
+      command: "cfsd-logic-aimpoint --cid=253 --previewTime=0.5 --verbose"

src/cfsd-logic-pathplanner.cpp

@@ -41,11 +41,10 @@ int32_t main(int32_t argc, char **argv) {
     bool const verbose{commandlineArguments.count("verbose") != 0};
     uint32_t localPathSenderId = 2601;
 
-    Collector collector(commandlineArguments);
-
     cluon::OD4Session od4{
       static_cast<uint16_t>(std::stoi(commandlineArguments["cid"]))};
-
+    Collector collector(od4, commandlineArguments);
+    
     auto onObjectFrameStart{[&planner = collector](
           cluon::data::Envelope &&envelope)
     {
@@ -88,6 +87,12 @@ int32_t main(int32_t argc, char **argv) {
       planner.getEquilibrioception(envelope);
     }};
 
+    auto onAimpoint{[&planner = collector](
+        cluon::data::Envelope &&envelope)
+    {
+      planner.getAimpoint(envelope);
+    }};
+    
     auto atFrequency{[&od4, &verbose, &collector, &localPathSenderId]() -> bool
       {
         {
@@ -144,6 +149,9 @@ int32_t main(int32_t argc, char **argv) {
         onObjectPosition);
     od4.dataTrigger(opendlv::logic::sensation::Equilibrioception::ID(),
         onEquilibrioception);
+        
+    od4.dataTrigger(opendlv::logic::action::AimPoint::ID(),
+        onAimpoint);
     od4.timeTrigger(std::stoi(commandlineArguments["freq"]), atFrequency);
 
     retCode = 0;

src/collector.cpp

@@ -54,7 +54,8 @@ Eigen::ArrayXXf getDisplacementTranslation(std::vector<Point2D> pastCones, std::
   return vec;
 }
 /*-------------------------------------------------------*/
-Collector::Collector(std::map<std::string, std::string> commandlineArguments) :
+Collector::Collector(cluon::OD4Session &od4, std::map<std::string, std::string> commandlineArguments) :
+    m_od4(od4),
     m_pastBlue{},
     m_pastYellow{},
     m_Cones{},
@@ -77,7 +78,9 @@ Collector::Collector(std::map<std::string, std::string> commandlineArguments) :
     currentCompleteFrameId(),
 
     frameStart(),
-    frameEnd()
+    frameEnd(),
+    
+    currentAim()
 
 {
   m_verbose = static_cast<bool>(commandlineArguments.count("verbose") != 0);
@@ -595,6 +598,13 @@ void Collector::ShowResult(std::vector<Cone> blue, std::vector<Cone> yellow, std
     }
   }
 
+  //Show aimpoint from Aimpoint module
+  //current aimpoint
+  int xt = int(currentAim.x * float(resultResize) + outWidth/2);
+  int yt = int(currentAim.y * float(resultResize));
+  cv::Scalar aimpointColor(0,0,255);
+  cv::circle(outImg, cv::Point(xt,-yt+outHeight-heightOffset), 4, aimpointColor, -1);
+  
   // Show result image
   cv::imshow("detectconelane-blue-yellow", outImg);
   cv::imshow("detectconelane-orange", outImgOrange);
@@ -706,3 +716,18 @@ void Collector::getEquilibrioception(cluon::data::Envelope envelope){
           << yawRate << std::endl;
       }
 }
+
+void Collector::getAimpoint(cluon::data::Envelope envelope){
+  opendlv::logic::action::AimPoint msg = 
+        cluon::extractMessage<opendlv::logic::action::AimPoint>(
+            std::move(envelope));
+
+      float angle = msg.azimuthAngle();
+      float distance = msg.distance();
+      currentAim.x = distance*cos(angle);
+      currentAim.y = distance*sin(angle);
+      if (m_verbose) {
+        std::cout << "Got AIMPOINT for object with id=" << 0 
+          << " and x=" << currentAim.x << " y=" << currentAim.y << std::endl;
+      }
+}

src/collector.hpp

@@ -42,7 +42,7 @@ struct Point2D {
 
 class Collector{
   public:
-    Collector(std::map<std::string, std::string>);
+    Collector(cluon::OD4Session &od4, std::map<std::string, std::string>);
     ~Collector() = default;
 
     // CFSD19 modification
@@ -61,19 +61,25 @@ class Collector{
     void ShowResult(std::vector<Cone> blue, std::vector<Cone> yellow, std::vector<Cone> orange, std::vector<Point2D> PredictedBlues, std::vector<Point2D> PredictedYellows);
 
   public:
+    cluon::OD4Session &m_od4;
+    //Containers for path planning
     std::vector<Point2D> m_pastBlue;
     std::vector<Point2D> m_pastYellow;
     std::queue<Cone*> m_Cones;
     std::queue<Cone> m_currentConeFrame;
     std::vector<Point2D> middlePath;
 
+    //Counters
     uint32_t m_collectorFrameCounter;
     uint32_t m_currentFrameCounter;
+    
+    //Flags and status
     Status m_status;
     bool m_isSkidpad;
     bool m_verbose;
     bool m_debug;
 
+    //Variables for creating cone frames
     std::mutex uncompleteFrameMutex;
     std::map<uint32_t, Cone> uncompleteFrame;
     uint32_t currentUncompleteFrameId;
@@ -84,13 +90,18 @@ class Collector{
 
     cluon::data::TimeStamp frameStart;
     cluon::data::TimeStamp frameEnd;
-    
+    Point2D currentAim;
+    //Function for creating cone frames
     void getObjectFrameStart(cluon::data::Envelope envelope);
     void getObjectFrameEnd(cluon::data::Envelope envelope);
     void getObject(cluon::data::Envelope envelope);
     void getObjectType(cluon::data::Envelope envelope);
     void getObjectPosition(cluon::data::Envelope envelope);
     void getEquilibrioception(cluon::data::Envelope envelope);
+    
+    //Visualiztion of aimpoints fro debugging
+    void getAimpoint(cluon::data::Envelope envelope);
+    
 };
 
 #endif