Updated teleop-frontend, fixed a bug with the colours in the map, and…

… refactored Parameters.cpp (introlab#36)

* Updated teleop-frontend, fixed a bug with the colours in the map, and refactored Parameters.cpp constructor for more uniformity

* Uniformized to US english (color/center)

map_image_generator/include/map_image_generator/Parameters.h

@@ -26,7 +26,7 @@ namespace map_image_generator
         bool m_drawOccupancyGrid;
         bool m_drawGlobalPath;
         bool m_drawRobot;
-        bool m_drawGoal;
+        bool m_drawGoals;
         bool m_drawLaserScan;
 
         cv::Vec3b m_wallColor;
@@ -67,7 +67,7 @@ namespace map_image_generator
         bool drawOccupancyGrid() const;
         bool drawGlobalPath() const;
         bool drawRobot() const;
-        bool drawGoal() const;
+        bool drawGoals() const;
         bool drawLaserScan() const;
 
         const cv::Vec3b& wallColor() const;
@@ -85,9 +85,6 @@ namespace map_image_generator
         int laserScanSize() const;       // pixel
 
     private:
-        static cv::Vec3b parseColorVec3b(const std::string& color);
-        static cv::Scalar parseColorScalar(const std::string& color);
-
         void validateParameters();
     };
 
@@ -134,7 +131,7 @@ namespace map_image_generator
 
     inline bool Parameters::drawRobot() const { return m_drawRobot; }
 
-    inline bool Parameters::drawGoal() const { return m_drawGoal; }
+    inline bool Parameters::drawGoals() const { return m_drawGoals; }
 
     inline bool Parameters::drawLaserScan() const { return m_drawLaserScan; }
 

map_image_generator/launch/map_image_generator.launch

@@ -21,7 +21,7 @@
     <arg name="x_origin_param" default="500"/>
     <arg name="y_origin_param" default="500"/>
     <arg name="robot_frame_param" default="base_footprint"/>
-    <arg name="draw_goal" default="true"/>
+    <arg name="draw_goals" default="true"/>
     <arg name="draw_robot" default="true"/>
     <arg name="centered_robot" default="true"/>
     <arg name="robot_vertical_offset" default="180"/>
@@ -49,7 +49,7 @@
         <param name="x_origin" type="int" value="$(arg x_origin_param)"/>
         <param name="y_origin" type="int" value="$(arg y_origin_param)"/>
         <param name="robot_frame" type="string" value="$(arg robot_frame_param)"/>
-        <param name="draw_goal" type="bool" value="$(arg draw_goal)" />
+        <param name="draw_goals" type="bool" value="$(arg draw_goals)" />
         <param name="draw_robot" type="bool" value="$(arg draw_robot)" />
         <param name="wall_color" type="string" value="0 224 225"/>
         <param name="free_space_color" type="string" value="24 39 52"/>

map_image_generator/src/MapImageGenerator.cpp

@@ -33,7 +33,7 @@ MapImageGenerator::MapImageGenerator(Parameters& parameters, ros::NodeHandle& no
             std::make_unique<RobotImageDrawer>(m_parameters, nodeHandle, m_tfListener));
     }
 
-    if (m_parameters.drawGoal())
+    if (m_parameters.drawGoals())
     {
         m_drawers.push_back(
             std::make_unique<GoalImageDrawer>(m_parameters, nodeHandle, m_tfListener));

map_image_generator/src/Parameters.cpp

@@ -3,62 +3,106 @@
 #include "map_image_generator/utils.h"
 
 #include <sstream>
+#include <type_traits>
 
 using namespace map_image_generator;
 using namespace std;
 
+namespace internal
+{
+    // Not defined for T: compilation fails if T is not cv::Scalar or cv::Vec3b
+    template <typename T>
+    T parseColor(const std::string& color);
+
+    template <>
+    cv::Scalar parseColor<cv::Scalar>(const std::string& color)
+    {
+        int r, g, b, a;
+        stringstream ss(color);
+        ss >> r >> g >> b >> a;
+
+        return {static_cast<double>(b), static_cast<double>(g), static_cast<double>(r),
+                static_cast<double>(a)};
+    }
+
+    template <>
+    cv::Vec3b parseColor<cv::Vec3b>(const std::string& color)
+    {
+        int r, g, b;
+        stringstream ss(color);
+        ss >> r >> g >> b;
+
+        return {static_cast<uchar>(b), static_cast<uchar>(g), static_cast<uchar>(r)};
+    }
+
+    // Compilation fails if T is not cv::Scalar or cv::Vec3b because of parseColor<T>
+    template <typename T>
+    T makeColor(const std::string& name, const std::string& defaultColor)
+    {
+        ros::NodeHandle nh{"~"};
+        std::string colorString;
+        nh.param<std::string>(name, colorString, defaultColor);
+        return parseColor<T>(colorString);
+    }
+
+    // Not defined if T is cv::Scalar or cv::Vec3b
+    template <typename T>
+    std::enable_if_t<
+        !std::is_same<T, cv::Scalar>::value && !std::is_same<T, cv::Vec3b>::value, T>
+    getParam(const std::string& name, const T& defaultValue)
+    {
+        ros::NodeHandle nh{"~"};
+        T value;
+        nh.param<T>(name, value, defaultValue);
+        return value;
+    }
+
+    // Defined only if T is cv::Scalar or cv::Vec3b
+    template <typename T>
+    std::enable_if_t<
+        std::is_same<T, cv::Scalar>::value || std::is_same<T, cv::Vec3b>::value, T>
+    getParam(const std::string& name, const std::string& defaultValue)
+    {
+        return makeColor<T>(name, defaultValue);
+    }
+}
+
 Parameters::Parameters(ros::NodeHandle& nodeHandle) : m_scaleFactor{1.0}
 {
-    nodeHandle.param("refresh_rate", m_refreshRate, 1.0);
-    nodeHandle.param("resolution", m_resolution, 50);
-    nodeHandle.param("width", m_width, 30);
-    nodeHandle.param("height", m_height, 15);
-    nodeHandle.param("x_origin", m_xOrigin, (m_width * m_resolution) / 2);
-    nodeHandle.param("y_origin", m_yOrigin, (m_height * m_resolution) / 2);
-    nodeHandle.param("robot_vertical_offset", m_robotVerticalOffset, 0);
-
-    nodeHandle.param("robot_frame_id", m_robotFrameId, string("base_footprint"));
-    nodeHandle.param("map_frame_id", m_mapFrameId, string("map"));
-
-    nodeHandle.param("draw_occupancy_grid", m_drawOccupancyGrid, true);
-    nodeHandle.param("draw_global_path", m_drawGlobalPath, true);
-    nodeHandle.param("draw_robot", m_drawRobot, true);
-    nodeHandle.param("draw_goal", m_drawGoal, true);
-    nodeHandle.param("draw_laser_scan", m_drawLaserScan, true);
-
-    string wallColorString;
-    string freeSpaceColorString;
-    string unknownSpaceColorString;
-    string globalPathColorString;
-    string robotColorString;
-    string goalColorString;
-    string laserScanColorString;
-    string textColorString;
-
-    nodeHandle.param("wall_color", wallColorString, string("0 0 0"));
-    nodeHandle.param("free_space_color", freeSpaceColorString, string("255 255 255"));
-    nodeHandle.param("unknown_space_color", unknownSpaceColorString,
-                     string("175 175 175"));
-    nodeHandle.param("global_path_color", globalPathColorString, string("0 255 0 255"));
-    nodeHandle.param("robot_color", robotColorString, string("0 0 255 255"));
-    nodeHandle.param("goal_color", goalColorString, string("0 175 0 255"));
-    nodeHandle.param("laser_scan_color", laserScanColorString, string("255 0 0 255"));
-    nodeHandle.param("text_color", textColorString, string("255 255 255"));
-
-    m_wallColor = parseColorVec3b(wallColorString);
-    m_freeSpaceColor = parseColorVec3b(freeSpaceColorString);
-    m_unknownSpaceColor = parseColorVec3b(unknownSpaceColorString);
-    m_globalPathColor = parseColorScalar(globalPathColorString);
-    m_robotColor = parseColorScalar(robotColorString);
-    m_goalColor = parseColorScalar(goalColorString);
-    m_laserScanColor = parseColorScalar(laserScanColorString);
-    m_textColor = parseColorScalar(textColorString);
-
-    nodeHandle.param("global_path_thickness", m_globalPathThickness, 3);
-    nodeHandle.param("robot_size", m_robotSize, 30);
-    nodeHandle.param("goal_size", m_goalSize, 20);
-    nodeHandle.param("laser_scan_size", m_laserScanSize, 6);
-    nodeHandle.param("centered_robot", m_centeredRobot, true);
+    using namespace internal;
+
+    m_refreshRate = getParam<double>("refresh_rate", 1.0);
+    m_resolution = getParam<int>("resolution", 50);
+    m_width = getParam<int>("width", 30);
+    m_height = getParam<int>("height", 15);
+    m_xOrigin = getParam<int>("x_origin", (m_width * m_resolution) / 2);
+    m_yOrigin = getParam<int>("y_origin", (m_height * m_resolution) / 2);
+    m_robotVerticalOffset = getParam<int>("robot_vertical_offset", 0);
+
+    m_robotFrameId = getParam<std::string>("robot_frame_id", "base_footprint");
+    m_mapFrameId = getParam<std::string>("map_frame_id", "map");
+
+    m_drawOccupancyGrid = getParam<bool>("draw_occupancy_grid", true);
+    m_drawGlobalPath = getParam<bool>("draw_global_path", true);
+    m_drawRobot = getParam<bool>("draw_robot", true);
+    m_drawGoals = getParam<bool>("draw_goals", true);
+    m_drawLaserScan = getParam<bool>("draw_laser_scan", true);
+
+    m_wallColor = getParam<cv::Vec3b>("wall_color", "0 0 0");
+    m_freeSpaceColor = getParam<cv::Vec3b>("free_space_color", "255 255 255");
+    m_unknownSpaceColor = getParam<cv::Vec3b>("unknown_space_color", "175 175 175");
+    m_globalPathColor = getParam<cv::Scalar>("global_path_color", "0 255 0 255");
+    m_robotColor = getParam<cv::Scalar>("robot_color", "0 0 255 255");
+    m_goalColor = getParam<cv::Scalar>("goal_color", "0 175 0 255");
+    m_laserScanColor = getParam<cv::Scalar>("laser_scan_color", "255 0 0 255");
+    m_textColor = getParam<cv::Scalar>("text_color", "255 255 255 255");
+
+    m_globalPathThickness = getParam<int>("global_path_thickness", 3);
+    m_robotSize = getParam<int>("robot_size", 30);
+    m_goalSize = getParam<int>("goal_size", 20);
+    m_laserScanSize = getParam<int>("laser_scan_size", 6);
+
+    m_centeredRobot = getParam<bool>("centered_robot", true);
 
     validateParameters();
 }
@@ -99,21 +143,3 @@ void Parameters::setCenteredRobot(bool centeredRobot)
     m_centeredRobot = centeredRobot;
     m_scaleFactor = 1.0;
 }
-
-cv::Vec3b Parameters::parseColorVec3b(const std::string& color)
-{
-    cv::Vec3b::value_type r, g, b;
-    stringstream ss(color);
-    ss >> r >> g >> b;
-
-    return {b, g, r};
-}
-
-cv::Scalar Parameters::parseColorScalar(const std::string& color)
-{
-    cv::Scalar::value_type r, g, b, a;
-    stringstream ss(color);
-    ss >> r >> g >> b >> a;
-
-    return {b, g, r, a};
-}

map_image_generator/src/drawers/OccupancyGridImageDrawer.cpp

@@ -80,7 +80,7 @@ void OccupancyGridImageDrawer::drawNotScaledOccupancyGridImage()
         for (auto x = 0; x < m_notScaledOccupancyGridImage.cols; x++)
         {
             int dataIndex = y * m_notScaledOccupancyGridImage.cols + x;
-            int dataValue = static_cast<uchar>(m_lastOccupancyGrid->data[dataIndex]);
+            int8_t dataValue = m_lastOccupancyGrid->data[dataIndex];
 
             cv::Point pixelPosition(x, y);
             if (dataValue == -1)
@@ -238,7 +238,7 @@ void OccupancyGridImageDrawer::drawOccupancyGridImage(cv::Mat& image)
     double heightBorder = 0.1 * outHeight;
     double widthBorder = 0.1 * outWidth;
 
-    // Map centre
+    // Map center
     double occupancyXOrigin = m_lastOccupancyGrid->info.origin.position.x;
     double occupancyYOrigin = m_lastOccupancyGrid->info.origin.position.y;
 
@@ -309,9 +309,9 @@ void OccupancyGridImageDrawer::drawOccupancyGridImageCenteredAroundRobot(cv::Mat
     using namespace map_image_generator;
 
     double rotationAngle = rad2deg(tf::getYaw(robotTransform->getRotation()));
-    cv::Point rotationCentre{robotCoordinates.x + padding.left,
+    cv::Point rotationCenter{robotCoordinates.x + padding.left,
                              robotCoordinates.y + padding.top};
-    rotateImageAboutPoint(paddedImage, rotationAngle, rotationCentre);
+    rotateImageAboutPoint(paddedImage, rotationAngle, rotationCenter);
 
     cv::Rect roi(cv::Point(restrictToPositive(robotPosition.left - (outWidth - 1) / 2
                                               + m_parameters.robotVerticalOffset()),

map_image_generator/src/utils.cpp

@@ -45,11 +45,11 @@ namespace map_image_generator
         geometry_msgs::Pose pose;
         int rows = parameters.height() * parameters.resolution();
         int cols = parameters.width() * parameters.resolution();
-        double centreY = rows / 2.0;
-        double centreX = cols / 2.0;
-        pose.position.x = -(y - centreY - parameters.robotVerticalOffset())
+        double centerY = rows / 2.0;
+        double centerX = cols / 2.0;
+        pose.position.x = -(y - centerY - parameters.robotVerticalOffset())
                           / static_cast<double>(parameters.resolution());
-        pose.position.y = -(x - centreX) / static_cast<double>(parameters.resolution());
+        pose.position.y = -(x - centerX) / static_cast<double>(parameters.resolution());
         pose.position.z = 0;
         pose.orientation = tf::createQuaternionMsgFromYaw(yaw);
         offsetYawByMinus90Degrees(pose);

opentera_webrtc_demos/launch/demo.launch

@@ -21,7 +21,7 @@
     <include file="$(find map_image_generator)/launch/map_image_generator.launch" >
         <arg name="refresh_rate_param" value="5"/>
         <arg name="input_global_path_topic" value="/move_base/NavfnROS/plan"/>
-        <arg name="draw_goal" value="true"/>
+        <arg name="draw_goals" value="true"/>
         <arg name="centered_robot" value="$(arg centered_robot)"/>
         <arg name="robot_vertical_offset" value="$(arg robot_vertical_offset)"/>