[Fawkes Git] branch/timn/laser-lines-frames: created (0.5.0-3084-g0fc8e41)

Tim Niemueller niemueller at kbsg.rwth-aachen.de
Tue Jul 12 09:52:13 CEST 2016


Changes have been pushed for the project "Fawkes Robotics Software Framework".

Gitweb: http://git.fawkesrobotics.org/fawkes.git
Trac:   http://trac.fawkesrobotics.org

The branch, timn/laser-lines-frames has been created
        at  0fc8e4150162a60ae8211f9238db6f5f770fe22b (commit)

http://git.fawkesrobotics.org/fawkes.git/timn/laser-lines-frames

- *Log* ---------------------------------------------------------------
commit d951b71774f982b3fb9236a7b60da79069c8e476
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Thu Jun 30 21:10:13 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Thu Jun 30 21:10:13 2016 +0200

    laser-lines: publish coordinate frames for the line end points
    
    This will align the Y axis of the frame to the direction of the line and
    have the X axis face towards the robot.

http://git.fawkesrobotics.org/fawkes.git/commit/d951b71
http://trac.fawkesrobotics.org/changeset/d951b71

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit c9d50b6de5255b7e7eb4774bf3d423a2cc457593
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Fri Jul 1 15:24:59 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Jul 1 15:24:59 2016 +0200

    laser-lines: separately publish laser lines and their average
    
    This is to better distinguish them in rviz. The lines and their averages
    are now published with the same base identifier (e.g., L_1) and a suffix
    for the average (_avg). Additionally, they are published with separate
    namespaces to easily allow enabling or disabling the one or the other.

http://git.fawkesrobotics.org/fawkes.git/commit/c9d50b6
http://trac.fawkesrobotics.org/changeset/c9d50b6

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
commit 0fc8e4150162a60ae8211f9238db6f5f770fe22b
Author:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
AuthorDate: Fri Jul 1 15:26:46 2016 +0200
Commit:     Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
CommitDate: Fri Jul 1 15:26:46 2016 +0200

    laser-lines: fix identifier offset for frames
    
    Publish the frames with the same number identifying the laser lines in
    the blackboard and visualization.

http://git.fawkesrobotics.org/fawkes.git/commit/0fc8e41
http://trac.fawkesrobotics.org/changeset/0fc8e41

- - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -


- *Summary* -----------------------------------------------------------
 src/plugins/laser-lines/laser-lines-thread.cpp |   42 +++++++++--------------
 src/plugins/laser-lines/laser-lines-thread.h   |    3 +-
 2 files changed, 19 insertions(+), 26 deletions(-)


- *Diffs* -------------------------------------------------------------

- *commit* d951b71774f982b3fb9236a7b60da79069c8e476 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Thu Jun 30 21:10:13 2016 +0200
Subject: laser-lines: publish coordinate frames for the line end points

 src/plugins/laser-lines/laser-lines-thread.cpp |   61 +++++++++++++++++++-----
 src/plugins/laser-lines/laser-lines-thread.h   |    9 ++--
 2 files changed, 54 insertions(+), 16 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp b/src/plugins/laser-lines/laser-lines-thread.cpp
index 9b5ba78..ad405db 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -62,7 +62,7 @@ using namespace fawkes;
 LaserLinesThread::LaserLinesThread()
   : Thread("LaserLinesThread", Thread::OPMODE_WAITFORWAKEUP),
     BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_SENSOR_PROCESS),
-    TransformAspect(TransformAspect::ONLY_LISTENER)
+    TransformAspect(TransformAspect::BOTH, "laser_lines")
 {
 }
 
@@ -257,10 +257,10 @@ LaserLinesThread::loop()
     //TimeWait::wait(50000);
 
     for (unsigned int i = 0; i < cfg_max_num_lines_; ++i) {
-      set_line(line_ifs_[i], false);
+	    set_line(i, line_ifs_[i], false);
       if(cfg_moving_avg_enabled_)
       {
-        set_line(line_avg_ifs_[i], false);
+	      set_line(i, line_avg_ifs_[i], false);
       }
     }
 
@@ -368,10 +368,10 @@ LaserLinesThread::loop()
     const LineInfo &info_avg = linfos_filtered[i];
 
     if (line_if_idx < cfg_max_num_lines_) {
-      set_line(line_ifs_[line_if_idx], true, finput_->header.frame_id, info);
+	    set_line(line_if_idx, line_ifs_[line_if_idx], true, finput_->header.frame_id, info);
       if(cfg_moving_avg_enabled_)
       {
-        set_line(line_avg_ifs_[line_if_idx], true, finput_->header.frame_id, info_avg);
+        set_line(line_if_idx, line_avg_ifs_[line_if_idx], true, finput_->header.frame_id, info_avg);
       }
       line_if_idx++;
     }
@@ -394,10 +394,10 @@ LaserLinesThread::loop()
   }
 
   for (unsigned int i = line_if_idx; i < cfg_max_num_lines_; ++i) {
-    set_line(line_ifs_[i], false);
+	  set_line(i, line_ifs_[i], false);
     if(cfg_moving_avg_enabled_)
     {
-      set_line(line_avg_ifs_[i], false);
+	    set_line(i, line_avg_ifs_[i], false);
     }
   }
 
@@ -439,10 +439,11 @@ LaserLinesThread::loop()
 
 
 void
-LaserLinesThread::set_line(fawkes::LaserLineInterface *iface,
-			   bool is_visible,
-			   const std::string &frame_id,
-			   const LineInfo &info)
+LaserLinesThread::set_line(unsigned int idx,
+                           fawkes::LaserLineInterface *iface,
+                           bool is_visible,
+                           const std::string &frame_id,
+                           const LineInfo &info)
 {
   int visibility_history = iface->visibility_history();
   if (is_visible) {
@@ -474,6 +475,42 @@ LaserLinesThread::set_line(fawkes::LaserLineInterface *iface,
     iface->set_length(info.length);
     iface->set_end_point_1(if_end_point_1);
     iface->set_end_point_2(if_end_point_2);
+
+    // this makes the usual assumption that the laser data is in the X-Y plane
+    fawkes::Time now(clock);  
+    std::string frame_name_1, frame_name_2;
+    char *tmp;
+    if (asprintf(&tmp, "laser_line_%u_e1", idx) != -1) {
+	    frame_name_1 = tmp;
+	    free(tmp);
+    }
+    if (asprintf(&tmp, "laser_line_%u_e2", idx) != -1) {
+	    frame_name_2 = tmp;
+	    free(tmp);
+    }
+    if (frame_name_1 != "" && frame_name_2 != "") {
+	    Eigen::Vector3f bp_unit = info.base_point / info.base_point.norm();
+	    double dotprod = Eigen::Vector3f::UnitX().dot(bp_unit);
+	    double angle = acos(dotprod) + M_PI;
+
+	    if (info.base_point[1] < 0.)  angle = fabs(angle) * -1.;
+
+	    tf::Transform t1(tf::Quaternion(tf::Vector3(0,0,1), angle),
+	                     tf::Vector3(info.end_point_1[0], info.end_point_1[1], info.end_point_1[2]));
+	    tf::Transform t2(tf::Quaternion(tf::Vector3(0,0,1), angle),
+	                     tf::Vector3(info.end_point_2[0], info.end_point_2[1], info.end_point_2[2]));
+	    
+	    try {
+		    tf_publisher->send_transform(t1, now, frame_id, frame_name_1);
+		    tf_publisher->send_transform(t2, now, frame_id, frame_name_2);
+	    } catch (Exception &e) {
+		    logger->log_warn(name(), "Failed to publish laser_line_%u_* transforms, exception follows", idx);
+		    logger->log_warn(name(), e);
+	    }
+    } else {
+	    logger->log_warn(name(), "Failed to determine frame names");
+    }
+
   } else {
     if (visibility_history <= 0) {
       iface->set_visibility_history(visibility_history - 1);
@@ -489,7 +526,7 @@ LaserLinesThread::set_line(fawkes::LaserLineInterface *iface,
       iface->set_frame_id("");
     }
   }
-  iface->write();  
+  iface->write();
 }
 
 
diff --git a/src/plugins/laser-lines/laser-lines-thread.h b/src/plugins/laser-lines/laser-lines-thread.h
index b9745eb..53ab5b0 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -97,10 +97,11 @@ class LaserLinesThread
 
  private:
 
-  void set_line(fawkes::LaserLineInterface *iface,
-		bool is_visible,
-		const std::string &frame_id = "",
-		const LineInfo &info = LineInfo());
+  void set_line(unsigned int idx,
+                fawkes::LaserLineInterface *iface,
+                bool is_visible,
+                const std::string &frame_id = "",
+                const LineInfo &info = LineInfo());
 
 
 #ifdef HAVE_VISUAL_DEBUGGING

- *commit* c9d50b6de5255b7e7eb4774bf3d423a2cc457593 - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Fri Jul 1 15:24:59 2016 +0200
Subject: laser-lines: separately publish laser lines and their average

 src/plugins/laser-lines/laser-lines-thread.cpp |   36 +++++++++--------------
 src/plugins/laser-lines/laser-lines-thread.h   |    3 +-
 2 files changed, 16 insertions(+), 23 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp b/src/plugins/laser-lines/laser-lines-thread.cpp
index ad405db..54d9182 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -402,19 +402,10 @@ LaserLinesThread::loop()
   }
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  std::vector<LineInfo> to_publish;
-  for(LineInfo info: linfos)
-  {
-    to_publish.push_back(info);
-  }
-  if(cfg_moving_avg_enabled_)
-  {
-    for(LineInfo info: linfos_filtered)
-    {
-      to_publish.push_back(info);
-    }
+  publish_visualization(linfos, "laser_lines");
+  if(cfg_moving_avg_enabled_) {
+	  publish_visualization(linfos_filtered, "laser_lines_moving_average", "_avg");
   }
-  publish_visualization(to_publish);
 #endif
 
   //*lines_ = *tmp_lines;
@@ -532,7 +523,8 @@ LaserLinesThread::set_line(unsigned int idx,
 
 #ifdef HAVE_VISUAL_DEBUGGING
 void
-LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
+LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos,
+                                        std::string marker_namespace, std::string name_suffix)
 {
   visualization_msgs::MarkerArray m;
   unsigned int idnum = 0;
@@ -544,7 +536,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
     visualization_msgs::Marker basevec;
     basevec.header.frame_id = finput_->header.frame_id;
     basevec.header.stamp = ros::Time::now();
-    basevec.ns = "laser_lines";
+    basevec.ns = marker_namespace;
     basevec.id = idnum++;
     basevec.type = visualization_msgs::Marker::ARROW;
     basevec.action = visualization_msgs::Marker::ADD;
@@ -565,7 +557,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
     visualization_msgs::Marker dirvec;
     dirvec.header.frame_id = finput_->header.frame_id;
     dirvec.header.stamp = ros::Time::now();
-    dirvec.ns = "laser_lines";
+    dirvec.ns = marker_namespace;
     dirvec.id = idnum++;
     dirvec.type = visualization_msgs::Marker::ARROW;
     dirvec.action = visualization_msgs::Marker::ADD;
@@ -588,7 +580,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
     visualization_msgs::Marker testvec;
     testvec.header.frame_id = finput_->header.frame_id;
     testvec.header.stamp = ros::Time::now();
-    testvec.ns = "laser_lines";
+    testvec.ns = marker_namespace;
     testvec.id = idnum++;
     testvec.type = visualization_msgs::Marker::ARROW;
     testvec.action = visualization_msgs::Marker::ADD;
@@ -609,7 +601,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
     m.markers.push_back(testvec);
 
     char *tmp;
-    if (asprintf(&tmp, "L_%zu", i+1) != -1) {
+    if (asprintf(&tmp, "L_%zu%s", i+1, name_suffix.c_str()) != -1) {
       // Copy to get memory freed on exception
       std::string id = tmp;
       free(tmp);
@@ -617,7 +609,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
       visualization_msgs::Marker text;
       text.header.frame_id = finput_->header.frame_id;
       text.header.stamp = ros::Time::now();
-      text.ns = "laser_lines";
+      text.ns = marker_namespace;
       text.id = idnum++;
       text.type = visualization_msgs::Marker::TEXT_VIEW_FACING;
       text.action = visualization_msgs::Marker::ADD;
@@ -637,7 +629,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
       visualization_msgs::Marker sphere_ep_1;
       sphere_ep_1.header.frame_id = finput_->header.frame_id;
       sphere_ep_1.header.stamp = ros::Time::now();
-      sphere_ep_1.ns = "laser_lines";
+      sphere_ep_1.ns = marker_namespace;
       sphere_ep_1.id = idnum++;
       sphere_ep_1.type = visualization_msgs::Marker::SPHERE;
       sphere_ep_1.action = visualization_msgs::Marker::ADD;
@@ -658,7 +650,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
       visualization_msgs::Marker sphere_ep_2;
       sphere_ep_2.header.frame_id = finput_->header.frame_id;
       sphere_ep_2.header.stamp = ros::Time::now();
-      sphere_ep_2.ns = "laser_lines";
+      sphere_ep_2.ns = marker_namespace;
       sphere_ep_2.id = idnum++;
       sphere_ep_2.type = visualization_msgs::Marker::SPHERE;
       sphere_ep_2.action = visualization_msgs::Marker::ADD;
@@ -679,7 +671,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
       visualization_msgs::Marker lineseg;
       lineseg.header.frame_id = finput_->header.frame_id;
       lineseg.header.stamp = ros::Time::now();
-      lineseg.ns = "laser_lines";
+      lineseg.ns = marker_namespace;
       lineseg.id = idnum++;
       lineseg.type = visualization_msgs::Marker::LINE_LIST;
       lineseg.action = visualization_msgs::Marker::ADD;
@@ -705,7 +697,7 @@ LaserLinesThread::publish_visualization(const std::vector<LineInfo> &linfos)
     visualization_msgs::Marker delop;
     delop.header.frame_id = finput_->header.frame_id;
     delop.header.stamp = ros::Time::now();
-    delop.ns = "laser_lines";
+    delop.ns = marker_namespace;
     delop.id = i;
     delop.action = visualization_msgs::Marker::DELETE;
     m.markers.push_back(delop);
diff --git a/src/plugins/laser-lines/laser-lines-thread.h b/src/plugins/laser-lines/laser-lines-thread.h
index 53ab5b0..136cf13 100644
--- a/src/plugins/laser-lines/laser-lines-thread.h
+++ b/src/plugins/laser-lines/laser-lines-thread.h
@@ -105,7 +105,8 @@ class LaserLinesThread
 
 
 #ifdef HAVE_VISUAL_DEBUGGING
-  void publish_visualization(const std::vector<LineInfo> &linfos);
+  void publish_visualization(const std::vector<LineInfo> &linfos,
+                             std::string marker_namespace, std::string name_suffix = "");
 #endif
 
  private:

- *commit* 0fc8e4150162a60ae8211f9238db6f5f770fe22b - - - - - - - - - -
Author:  Tim Niemueller <niemueller at kbsg.rwth-aachen.de>
Date:    Fri Jul 1 15:26:46 2016 +0200
Subject: laser-lines: fix identifier offset for frames

 src/plugins/laser-lines/laser-lines-thread.cpp |    6 +++---
 1 files changed, 3 insertions(+), 3 deletions(-)

_Diff for modified files_:
diff --git a/src/plugins/laser-lines/laser-lines-thread.cpp b/src/plugins/laser-lines/laser-lines-thread.cpp
index 54d9182..787b75a 100644
--- a/src/plugins/laser-lines/laser-lines-thread.cpp
+++ b/src/plugins/laser-lines/laser-lines-thread.cpp
@@ -471,11 +471,11 @@ LaserLinesThread::set_line(unsigned int idx,
     fawkes::Time now(clock);  
     std::string frame_name_1, frame_name_2;
     char *tmp;
-    if (asprintf(&tmp, "laser_line_%u_e1", idx) != -1) {
+    if (asprintf(&tmp, "laser_line_%u_e1", idx+1) != -1) {
 	    frame_name_1 = tmp;
 	    free(tmp);
     }
-    if (asprintf(&tmp, "laser_line_%u_e2", idx) != -1) {
+    if (asprintf(&tmp, "laser_line_%u_e2", idx+1) != -1) {
 	    frame_name_2 = tmp;
 	    free(tmp);
     }
@@ -495,7 +495,7 @@ LaserLinesThread::set_line(unsigned int idx,
 		    tf_publisher->send_transform(t1, now, frame_id, frame_name_1);
 		    tf_publisher->send_transform(t2, now, frame_id, frame_name_2);
 	    } catch (Exception &e) {
-		    logger->log_warn(name(), "Failed to publish laser_line_%u_* transforms, exception follows", idx);
+		    logger->log_warn(name(), "Failed to publish laser_line_%u_* transforms, exception follows", idx+1);
 		    logger->log_warn(name(), e);
 	    }
     } else {




-- 
Fawkes Robotics Framework                 http://www.fawkesrobotics.org


More information about the fawkes-commits mailing list