Merge branch 'fix/working_on_navigation' into 'master'

Fixed missing z when transforming point; Added missing return value for...

See merge request !1

include/homer_nav_libs/tools.h

@@ -25,7 +25,7 @@ namespace map_tools
  * @param resolution Resolution of the map
  * @return Cell position of the point
  */
-Eigen::Vector2i toMapCoords(geometry_msgs::Point p, geometry_msgs::Pose origin,
+Eigen::Vector2i toMapCoords(const geometry_msgs::Point& p, const geometry_msgs::Pose& origin,
                             float resolution)
 {
   int x_idx = (p.x - origin.position.x) / resolution + 0.51;
@@ -163,16 +163,16 @@ geometry_msgs::Point transformPoint(geometry_msgs::Point point,
   try
   {
     listener.transformPoint(to_frame, stamp, pin, "/map", pout);
-    return pout.point;
   }
   catch (tf::TransformException ex)
   {
     ROS_ERROR("%s", ex.what());
   }
+  return pout.point;
 }
 
-geometry_msgs::Point transformPoint(const geometry_msgs::Point point,
-                                    tf::StampedTransform transform)
+geometry_msgs::Point transformPoint(const geometry_msgs::Point& point,
+                                    const tf::StampedTransform& transform)
 {
   geometry_msgs::Point point_out;
   tf::Vector3 pin;
@@ -185,11 +185,12 @@ geometry_msgs::Point transformPoint(const geometry_msgs::Point point,
 
   point_out.x = pout.x();
   point_out.y = pout.y();
+  point_out.z = pout.z();
 
   return point_out;
 }
-geometry_msgs::Point transformPoint(const geometry_msgs::Point point,
-                                    tf::Transform transform)
+geometry_msgs::Point transformPoint(const geometry_msgs::Point& point,
+                                    const tf::Transform& transform)
 {
   geometry_msgs::Point point_out;
   tf::Vector3 pin;
@@ -230,12 +231,12 @@ geometry_msgs::Point transformPoint(geometry_msgs::Point point,
   try
   {
     listener.transformPoint(to_frame, time, pin, "/map", pout);
-    return pout.point;
   }
   catch (tf::TransformException ex)
   {
     ROS_ERROR("%s", ex.what());
   }
+  return pout.point;
 }
 
 /**