diff --git a/doc/Development/rviz.rviz b/doc/Development/rviz.rviz index d40a101..1cd9a9b 100644 --- a/doc/Development/rviz.rviz +++ b/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,7 +191,7 @@ 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 @@ -99,17 +199,17 @@ Visualization Manager: 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: - Yaw: 1.1404004096984863 + Yaw: 0.715400755405426 Saved: ~ Window Geometry: Displays: diff --git a/nodes/SonarArrayDriverNode/doc/SonarArrayDriverNodeClassDiagram.puml b/nodes/SonarArrayDriverNode/doc/SonarArrayDriverNodeClassDiagram.puml index 3236abb..167df61 100644 --- a/nodes/SonarArrayDriverNode/doc/SonarArrayDriverNodeClassDiagram.puml +++ b/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 new_commandmsg(eros::command msg) +std::vector check_programvariables() +cleanup() + +void set_sonar_config(std::vector _sonar_config) + +void set_comm_port(std::string _comm_port) -ISonarArrayNodeDriver driver } diff --git a/nodes/SonarArrayDriverNode/doc/output/SonarArrayDriverNodeClassDiagram.png b/nodes/SonarArrayDriverNode/doc/output/SonarArrayDriverNodeClassDiagram.png index 4166c47..ea0e9a3 100644 Binary files a/nodes/SonarArrayDriverNode/doc/output/SonarArrayDriverNodeClassDiagram.png and b/nodes/SonarArrayDriverNode/doc/output/SonarArrayDriverNodeClassDiagram.png differ diff --git a/nodes/SonarArrayDriverNode/launch/app_sonar_array.launch b/nodes/SonarArrayDriverNode/launch/app_sonar_array.launch index 00da064..f8349b6 100644 --- a/nodes/SonarArrayDriverNode/launch/app_sonar_array.launch +++ b/nodes/SonarArrayDriverNode/launch/app_sonar_array.launch @@ -1,37 +1,34 @@ - + - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/nodes/SonarArrayDriverNode/launch/sonar_array_driver_node.launch b/nodes/SonarArrayDriverNode/launch/sonar_array_driver_node.launch index b48ca7c..8b48465 100644 --- a/nodes/SonarArrayDriverNode/launch/sonar_array_driver_node.launch +++ b/nodes/SonarArrayDriverNode/launch/sonar_array_driver_node.launch @@ -3,8 +3,11 @@ + + + @@ -12,5 +15,6 @@ + diff --git a/nodes/SonarArrayDriverNode/launch/sonar_config.yaml b/nodes/SonarArrayDriverNode/launch/sonar_config.yaml new file mode 100644 index 0000000..bf922b3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/nodes/SonarArrayDriverNode/node/SonarArrayDriverNode.cpp b/nodes/SonarArrayDriverNode/node/SonarArrayDriverNode.cpp index 3a06313..2f171a8 100644 --- a/nodes/SonarArrayDriverNode/node/SonarArrayDriverNode.cpp +++ b/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 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"; diff --git a/nodes/SonarArrayDriverNode/node/SonarArrayDriverNode.h b/nodes/SonarArrayDriverNode/node/SonarArrayDriverNode.h index 5e9c440..76bc071 100644 --- a/nodes/SonarArrayDriverNode/node/SonarArrayDriverNode.h +++ b/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 system_command_action_server; diff --git a/nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.cpp b/nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.cpp index 1f25ec2..0ca88da 100644 --- a/nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.cpp +++ b/nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.cpp @@ -12,26 +12,20 @@ eros::eros_diagnostic::Diagnostic SonarArrayDriverNodeProcess::finish_initializa std::vector 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; } diff --git a/nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.h b/nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.h index 811bbd7..62a69b7 100644 --- a/nodes/SonarArrayDriverNode/node/SonarArrayDriverNodeProcess.h +++ b/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 _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 new_commandmsg(eros::command msg); @@ -49,6 +55,8 @@ class SonarArrayDriverNodeProcess : public eros::BaseNodeProcess private: bool enable_mock{false}; + std::vector sonar_config; + std::string comm_port; ISonarArrayNodeDriver* driver; // Enable during AB#1452 }; } // namespace sonar_array diff --git a/nodes/SonarArrayDriverNode/node/test/mock_sonar_config.yaml b/nodes/SonarArrayDriverNode/node/test/mock_sonar_config.yaml new file mode 100644 index 0000000..bf922b3 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/nodes/SonarArrayDriverNode/node/test/test_SonarArrayDriverNode.test b/nodes/SonarArrayDriverNode/node/test/test_SonarArrayDriverNode.test index e0cc61f..461838b 100644 --- a/nodes/SonarArrayDriverNode/node/test/test_SonarArrayDriverNode.test +++ b/nodes/SonarArrayDriverNode/node/test/test_SonarArrayDriverNode.test @@ -6,6 +6,7 @@ +