Us 1491 node driver user config

doc/Development/rviz.rviz

@@ -8,6 +8,7 @@ Panels:
         - /Status1
         - /Grid1
         - /Range1
+        - /TF1
       Splitter Ratio: 0.5
     Tree Height: 719
   - Class: rviz/Selection
@@ -63,6 +64,105 @@ Visualization Manager:
       Topic: /robot/sonar
       Unreliable: false
       Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Filter (blacklist): ""
+      Filter (whitelist): ""
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        sonar0:
+          Value: true
+        sonar1:
+          Value: true
+        sonar10:
+          Value: true
+        sonar11:
+          Value: true
+        sonar12:
+          Value: true
+        sonar13:
+          Value: true
+        sonar14:
+          Value: true
+        sonar15:
+          Value: true
+        sonar16:
+          Value: true
+        sonar17:
+          Value: true
+        sonar18:
+          Value: true
+        sonar19:
+          Value: true
+        sonar2:
+          Value: true
+        sonar3:
+          Value: true
+        sonar4:
+          Value: true
+        sonar5:
+          Value: true
+        sonar6:
+          Value: true
+        sonar7:
+          Value: true
+        sonar8:
+          Value: true
+        sonar9:
+          Value: true
+        sonar_array:
+          Value: true
+      Marker Alpha: 1
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: false
+      Tree:
+        sonar_array:
+          sonar0:
+            {}
+          sonar1:
+            {}
+          sonar10:
+            {}
+          sonar11:
+            {}
+          sonar12:
+            {}
+          sonar13:
+            {}
+          sonar14:
+            {}
+          sonar15:
+            {}
+          sonar16:
+            {}
+          sonar17:
+            {}
+          sonar18:
+            {}
+          sonar19:
+            {}
+          sonar2:
+            {}
+          sonar3:
+            {}
+          sonar4:
+            {}
+          sonar5:
+            {}
+          sonar6:
+            {}
+          sonar7:
+            {}
+          sonar8:
+            {}
+          sonar9:
+            {}
+      Update Interval: 0
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -91,25 +191,25 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 6.89558219909668
+      Distance: 3.814835786819458
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.05999999865889549
         Stereo Focal Distance: 1
         Swap Stereo Eyes: false
         Value: false
       Field of View: 0.7853981852531433
       Focal Point:
-        X: 1.4072648286819458
-        Y: -0.33929795026779175
-        Z: 0.3396908640861511
+        X: -0.05515009164810181
+        Y: 0.5142452716827393
+        Z: 0.6449169516563416
       Focal Shape Fixed Size: true
       Focal Shape Size: 0.05000000074505806
       Invert Z Axis: false
       Name: Current View
       Near Clip Distance: 0.009999999776482582
-      Pitch: -0.024601440876722336
+      Pitch: 0.6553981900215149
       Target Frame: <Fixed Frame>
-      Yaw: 1.1404004096984863
+      Yaw: 0.715400755405426
     Saved: ~
 Window Geometry:
   Displays:

nodes/SonarArrayDriverNode/doc/SonarArrayDriverNodeClassDiagram.puml

@@ -36,7 +36,7 @@ package sonar_array {
       +bool changenodestate_service(eros::srv_change_nodestate::Request& req, eros::srv_change_nodestate::Response& res)
       +system_commandAction_Callback(const eros::system_commandGoalConstPtr& goal)
       +command_Callback(const eros::command::ConstPtr& t_msg)
-
+      -bool read_sonar_config();
       -SonarArrayDriverNodeProcess* process;
   }
   class SonarArrayDriverNodeProcess #Orange {
@@ -46,6 +46,8 @@ package sonar_array {
       +std::vector<eros::eros_diagnostic::Diagnostic> new_commandmsg(eros::command msg)
       +std::vector<eros::eros_diagnostic::Diagnostic> check_programvariables()
       +cleanup()
+      +void set_sonar_config(std::vector<sensor_msgs::Range> _sonar_config) 
+      +void set_comm_port(std::string _comm_port)
       -ISonarArrayNodeDriver driver
 
   }

nodes/SonarArrayDriverNode/launch/app_sonar_array.launch

@@ -1,37 +1,34 @@
 <?xml version="1.0"?>
 <launch>
-<arg name="enable_mock" default="False"/>
+<arg name="enable_mock" default="True"/>
 <arg name="robot_namespace" default="/"/>
 <arg name="verbosity_level" default="NOTICE"/> 
 <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find sonar_array)/doc/Development/rviz.rviz"/>
-<node name="sonar_array_driver_node" pkg="sonar_array" type="sonar_array_driver_node"  output="screen" clear_params="true">
-        <param name="enable_mock"               value="$(arg enable_mock)"/>
-        <param name="robot_namespace"           value="$(arg robot_namespace)"/>
-        <param name="startup_delay"             value="0.0"/>
-	<param name="verbosity_level"           value="$(arg verbosity_level)"/>
-        <param name="require_pps_to_start"      value="false"/>    
-        <param name="loop1_rate"                value="50"/> 
-        <param name="loop2_rate"                value="10"/>
-        <param name="loop3_rate"                value="5"/>
-</node>
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar0_broadcaster" args="1 0 0 0 -0.2618 0 sonar_array sonar0" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar1_broadcaster" args="1 0 0 0.314 -0.2618 0 sonar_array sonar1" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar2_broadcaster" args="1 0 0 0.628 -0.2618 0 sonar_array sonar2" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar3_broadcaster" args="1 0 0 .942 -0.2618 0 sonar_array sonar3" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar4_broadcaster" args="1 0 0 1.256 -0.2618 0 sonar_array sonar4" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar5_broadcaster" args="1 0 0 1.57 -0.2618 0 sonar_array sonar5" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar6_broadcaster" args="1 0 0 1.884 -0.2618 0 sonar_array sonar6" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar7_broadcaster" args="1 0 0 2.199 -0.2618 0 sonar_array sonar7" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar8_broadcaster" args="1 0 0 2.513 -0.2618 0 sonar_array sonar8" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar9_broadcaster" args="1 0 0 2.827 -0.2618 0 sonar_array sonar9" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar10_broadcaster" args="1 0 0 3.141 -0.2618 0 sonar_array sonar10" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar11_broadcaster" args="1 0 0 3.455 -0.2618 0 sonar_array sonar11" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar12_broadcaster" args="1 0 0 3.769 -0.2618 0 sonar_array sonar12" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar13_broadcaster" args="1 0 0 4.084 -0.2618 0 sonar_array sonar13" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar14_broadcaster" args="1 0 0 4.398 -0.2618 0 sonar_array sonar14" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar15_broadcaster" args="1 0 0 4.712 -0.2618 0 sonar_array sonar15" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar16_broadcaster" args="1 0 0 5.026 -0.2618 0 sonar_array sonar16" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar17_broadcaster" args="1 0 0 5.34 -0.2618 0 sonar_array sonar17" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar18_broadcaster" args="1 0 0 5.654 -0.2618 0 sonar_array sonar18" />
-<node pkg="tf2_ros" type="static_transform_publisher" name="sonar19_broadcaster" args="1 0 0 5.969 -0.2618 0 sonar_array sonar19" />
+<include file="$(find sonar_array)/nodes/SonarArrayDriverNode/launch/sonar_array_driver_node.launch">
+        <arg name="enable_mock"               value="$(arg enable_mock)"/>
+        <arg name="robot_namespace"           value="$(arg robot_namespace)"/>
+        <arg name="comm_port"                 value="/dev/ttyUSB0"/>
+        <arg name="sonar_config"              value="$(find sonar_array)/nodes/SonarArrayDriverNode/launch/sonar_config.yaml"/>
+        <arg name="verbosity_level"           value="$(arg verbosity_level)"/>
+</include>
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar0_broadcaster" args="0 0 1 0 -0.2618 0 sonar_array sonar0" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar1_broadcaster" args="0 0 1 0.314 -0.2618 0 sonar_array sonar1" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar2_broadcaster" args="0 0 1 0.628 -0.2618 0 sonar_array sonar2" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar3_broadcaster" args="0 0 1 .942 -0.2618 0 sonar_array sonar3" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar4_broadcaster" args="0 0 1 1.256 -0.2618 0 sonar_array sonar4" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar5_broadcaster" args="0 0 1 1.57 -0.2618 0 sonar_array sonar5" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar6_broadcaster" args="0 0 1 1.884 -0.2618 0 sonar_array sonar6" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar7_broadcaster" args="0 0 1 2.199 -0.2618 0 sonar_array sonar7" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar8_broadcaster" args="0 0 1 2.513 -0.2618 0 sonar_array sonar8" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar9_broadcaster" args="0 0 1 2.827 -0.2618 0 sonar_array sonar9" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar10_broadcaster" args="0 0 1 3.141 -0.2618 0 sonar_array sonar10" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar11_broadcaster" args="0 0 1 3.455 -0.2618 0 sonar_array sonar11" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar12_broadcaster" args="0 0 1 3.769 -0.2618 0 sonar_array sonar12" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar13_broadcaster" args="0 0 1 4.084 -0.2618 0 sonar_array sonar13" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar14_broadcaster" args="0 0 1 4.398 -0.2618 0 sonar_array sonar14" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar15_broadcaster" args="0 0 1 4.712 -0.2618 0 sonar_array sonar15" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar16_broadcaster" args="0 0 1 5.026 -0.2618 0 sonar_array sonar16" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar17_broadcaster" args="0 0 1 5.34 -0.2618 0 sonar_array sonar17" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar18_broadcaster" args="0 0 1 5.654 -0.2618 0 sonar_array sonar18" />
+<node pkg="tf2_ros" type="static_transform_publisher" name="sonar19_broadcaster" args="0 0 1 5.969 -0.2618 0 sonar_array sonar19" />
 </launch>

nodes/SonarArrayDriverNode/launch/sonar_array_driver_node.launch

@@ -3,14 +3,18 @@
 <arg name="enable_mock" default="False"/>
 <arg name="robot_namespace" default="/"/>
 <arg name="verbosity_level" default="DEBUG"/> 
+<arg name="comm_port" default="/dev/ttyUSB0"/>
+<arg name="sonar_config" default="sonar_config.yaml"/>
 <node name="sonar_array_driver_node" pkg="sonar_array" type="sonar_array_driver_node"  output="screen" clear_params="true">
         <param name="enable_mock"               value="$(arg enable_mock)"/>
+        <param name="comm_port"                 value="$(arg comm_port)"/>
         <param name="robot_namespace"           value="$(arg robot_namespace)"/>
         <param name="startup_delay"             value="0.0"/>
 	<param name="verbosity_level"           value="$(arg verbosity_level)"/>
         <param name="require_pps_to_start"      value="false"/>    
         <param name="loop1_rate"                value="50"/> 
         <param name="loop2_rate"                value="10"/>
         <param name="loop3_rate"                value="5"/>
+        <rosparam command="load"  param="sonar_config" file="$(arg sonar_config)" />
 </node>
 </launch>

nodes/SonarArrayDriverNode/launch/sonar_config.yaml

@@ -0,0 +1,4 @@
+"sonar_count": 20
+"minimum_distance_m": 0.02
+"maximum_distance_m": 2.0
+"field_of_view_deg": 30

nodes/SonarArrayDriverNode/node/SonarArrayDriverNode.cpp

@@ -78,7 +78,11 @@ bool SonarArrayDriverNode::start() {
     diagnostic_types.push_back(eros::eros_diagnostic::DiagnosticType::COMMUNICATIONS);
     diagnostic_types.push_back(eros::eros_diagnostic::DiagnosticType::SENSORS);
     process->enable_diagnostics(diagnostic_types);
-    process->finish_initialization();
+    diagnostic = process->finish_initialization();
+    if (diagnostic.level >= eros::Level::Type::ERROR) {
+        logger->log_diagnostic(diagnostic);
+        return false;
+    }
     diagnostic = finish_initialization();
     logger->log_diagnostic(diagnostic);
     if (diagnostic.level > eros::Level::Type::WARN) {
@@ -114,9 +118,60 @@ eros::eros_diagnostic::Diagnostic SonarArrayDriverNode::read_launchparameters()
     if (enable_mock) {
         process->set_enable_mock(true);
     }
+    bool status = read_sonar_config();
+    if (status == false) {
+        diag = process->update_diagnostic(eros::eros_diagnostic::DiagnosticType::DATA_STORAGE,
+                                          eros::Level::Type::ERROR,
+                                          eros::eros_diagnostic::Message::INITIALIZING_ERROR,
+                                          "Unable to load Sonar Config.");
+        logger->log_diagnostic(diag);
+        return diag;
+    }
+
     get_logger()->log_notice("Configuration Files Loaded.");
     return diag;
 }
+bool SonarArrayDriverNode::read_sonar_config() {
+    int sonar_count = 0;
+    if (n->getParam("sonar_config/sonar_count", sonar_count) == false) {
+        get_logger()->log_error("Unable to fetch: sonar_count");
+        return false;
+    }
+    if (sonar_count == 0) {
+        return false;
+    }
+    double minimum_distance_m = 0.0;
+    if (n->getParam("sonar_config/minimum_distance_m", minimum_distance_m) == false) {
+        get_logger()->log_error("Unable to fetch: minimum_distance_m");
+        return false;
+    }
+    double maximum_distance_m = 0.0;
+    if (n->getParam("sonar_config/maximum_distance_m", maximum_distance_m) == false) {
+        get_logger()->log_error("Unable to fetch: maximum_distance_m");
+        return false;
+    }
+    double field_of_view_deg = 0.0;
+    if (n->getParam("sonar_config/field_of_view_deg", field_of_view_deg) == false) {
+        get_logger()->log_error("Unable to fetch: field_of_view_deg");
+        return false;
+    }
+    std::vector<sensor_msgs::Range> sonar_config;
+    sonar_config.resize(sonar_count);
+    for (std::size_t i = 0; i < sonar_config.size(); ++i) {
+        sonar_config.at(i).radiation_type = sensor_msgs::Range::ULTRASOUND;
+        sonar_config.at(i).field_of_view = field_of_view_deg * M_PI / 180.0;
+        sonar_config.at(i).min_range = minimum_distance_m;
+        sonar_config.at(i).max_range = maximum_distance_m;
+        sonar_config.at(i).header.frame_id = "sonar" + std::to_string(i);
+    }
+    process->set_sonar_config(sonar_config);
+    std::string comm_port = "";
+    if (n->getParam("comm_port", comm_port) == false) {
+        get_logger()->log_warn("Unable to fetch: comm_port");
+    }
+    process->set_comm_port(comm_port);
+    return true;
+}
 eros::eros_diagnostic::Diagnostic SonarArrayDriverNode::finish_initialization() {
     eros::eros_diagnostic::Diagnostic diag = diagnostic;
     std::string srv_nodestate_topic = "srv_nodestate_change";

nodes/SonarArrayDriverNode/node/SonarArrayDriverNode.h

@@ -74,6 +74,7 @@ class SonarArrayDriverNode : public eros::BaseNode
 
    private:
     eros::eros_diagnostic::Diagnostic read_launchparameters();
+    bool read_sonar_config();
     SonarArrayDriverNodeProcess* process;
     actionlib::SimpleActionServer<eros::system_commandAction> system_command_action_server;
 

nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.cpp

@@ -12,26 +12,20 @@ eros::eros_diagnostic::Diagnostic SonarArrayDriverNodeProcess::finish_initializa
     std::vector<sensor_msgs::Range> sonars;
 
     if (enable_mock) {
-        sonars.resize(20);
         logger->log_warn("Enabling Mock Sonary Array Node Driver.");
         driver = new MockSonarArrayNodeDriver();
     }
     else {
-        sonars.resize(1);
         driver = new SonarArrayNodeDriver();
     }
-
-    // Clean up this during AB#1491
-
-    for (std::size_t i = 0; i < sonars.size(); ++i) {
-        sonars.at(i).radiation_type = sensor_msgs::Range::ULTRASOUND;
-        sonars.at(i).field_of_view = 30.0 * M_PI / 180.0;
-        sonars.at(i).min_range = 0.02;  // meters
-        sonars.at(i).max_range = 2.0;   // meters
-        sonars.at(i).header.frame_id = "sonar" + std::to_string(i);
+    driver->init(diag, logger, sonar_config);
+    if (driver->set_comm_device(comm_port, B115200) == false) {
+        diag = update_diagnostic(eros::eros_diagnostic::DiagnosticType::COMMUNICATIONS,
+                                 eros::Level::Type::ERROR,
+                                 eros::eros_diagnostic::Message::INITIALIZING_ERROR,
+                                 "Unable to Initialize Comm Port: " + comm_port);
+        return diag;
     }
-    driver->init(diag, logger, sonars);
-    driver->set_comm_device("/dev/ttyUSB0", B115200);
 
     return diag;
 }

nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.h

@@ -28,6 +28,12 @@ class SonarArrayDriverNodeProcess : public eros::BaseNodeProcess
     void set_enable_mock(bool v) {
         enable_mock = v;
     }
+    void set_sonar_config(std::vector<sensor_msgs::Range> _sonar_config) {
+        sonar_config = _sonar_config;
+    }
+    void set_comm_port(std::string _comm_port) {
+        comm_port = _comm_port;
+    }
     void reset();
     eros::eros_diagnostic::Diagnostic update(double t_dt, double t_ros_time);
     std::vector<eros::eros_diagnostic::Diagnostic> new_commandmsg(eros::command msg);
@@ -49,6 +55,8 @@ class SonarArrayDriverNodeProcess : public eros::BaseNodeProcess
 
    private:
     bool enable_mock{false};
+    std::vector<sensor_msgs::Range> sonar_config;
+    std::string comm_port;
     ISonarArrayNodeDriver* driver;  // Enable during AB#1452
 };
 }  // namespace sonar_array

nodes/SonarArrayDriverNode/node/test/mock_sonar_config.yaml

@@ -0,0 +1,4 @@
+"sonar_count": 20
+"minimum_distance_m": 0.02
+"maximum_distance_m": 2.0
+"field_of_view_deg": 30