diff --git a/src/libs/nav2_util/CMakeLists.txt b/src/libs/nav2_util/CMakeLists.txt index 5a274cf4f1b4405455e3750aca6b4da3bc33d440..316cb1993ef365589354556c14bccba33d3ef5ce 100644 --- a/src/libs/nav2_util/CMakeLists.txt +++ b/src/libs/nav2_util/CMakeLists.txt @@ -50,7 +50,7 @@ add_library(pf_lib SHARED ) add_library(sensors_lib SHARED - src/sensors/amcl_sensor.cpp + src/sensors/sensor.cpp src/sensors/odom.cpp src/sensors/laser.cpp ) diff --git a/src/libs/nav2_util/include/nav2_util/sensors/laser.h b/src/libs/nav2_util/include/nav2_util/sensors/laser.h index 8270aa408dd48d6afc33b68af6f82bb67c836fdf..61faa0452346604206cdee74f7f82ceded410d52 100644 --- a/src/libs/nav2_util/include/nav2_util/sensors/laser.h +++ b/src/libs/nav2_util/include/nav2_util/sensors/laser.h @@ -29,7 +29,7 @@ #ifndef AMCL_LASER_H #define AMCL_LASER_H -#include "nav2_util/sensors/amcl_sensor.h" +#include "nav2_util/sensors/sensor.h" #include "nav2_util/map/map.h" namespace amcl @@ -43,7 +43,7 @@ typedef enum } laser_model_t; // Laser sensor data -class LaserData : public AMCLSensorData +class LaserData : public SensorData { public: LaserData () {ranges=NULL;}; @@ -56,7 +56,7 @@ class LaserData : public AMCLSensorData // Laseretric sensor model -class Laser : public AMCLSensor +class Laser : public Sensor { // Default constructor public: Laser(size_t max_beams, map_t* map); @@ -88,7 +88,7 @@ class Laser : public AMCLSensor // Update the filter based on the sensor model. Returns true if the // filter has been updated. - public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); + public: virtual bool UpdateSensor(pf_t *pf, SensorData *data); // Set the laser's pose after construction public: void SetLaserPose(pf_vector_t& laser_pose) diff --git a/src/libs/nav2_util/include/nav2_util/sensors/amcl_odom.h b/src/libs/nav2_util/include/nav2_util/sensors/odom.h similarity index 93% rename from src/libs/nav2_util/include/nav2_util/sensors/amcl_odom.h rename to src/libs/nav2_util/include/nav2_util/sensors/odom.h index 8a4e32d0c29306495d1fa237a9f9da6984a7e5fd..bf2276b00d6a1dd041b7628d7bc839ed9d59ce29 100644 --- a/src/libs/nav2_util/include/nav2_util/sensors/amcl_odom.h +++ b/src/libs/nav2_util/include/nav2_util/sensors/odom.h @@ -29,7 +29,7 @@ #ifndef AMCL_ODOM_H #define AMCL_ODOM_H -#include "nav2_util/sensors/amcl_sensor.h" +#include "nav2_util/sensors/sensor.h" #include "nav2_util/pf/pf_pdf.h" namespace amcl @@ -44,7 +44,7 @@ typedef enum } odom_model_t; // Odometric sensor data -class OdomData : public AMCLSensorData +class OdomData : public SensorData { // Odometric pose public: pf_vector_t pose; @@ -55,7 +55,7 @@ class OdomData : public AMCLSensorData // Odometric sensor model -class Odom : public AMCLSensor +class Odom : public Sensor { // Default constructor public: Odom(); @@ -80,7 +80,7 @@ class Odom : public AMCLSensor // Update the filter based on the action model. Returns true if the filter // has been updated. - public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); + public: virtual bool UpdateAction(pf_t *pf, SensorData *data); // Current data timestamp private: double time; diff --git a/src/libs/nav2_util/include/nav2_util/sensors/amcl_sensor.h b/src/libs/nav2_util/include/nav2_util/sensors/sensor.h similarity index 84% rename from src/libs/nav2_util/include/nav2_util/sensors/amcl_sensor.h rename to src/libs/nav2_util/include/nav2_util/sensors/sensor.h index 392f13621f57f515a5feb8d77499b2dd474d64b3..499837512a76fadaf9bbaf3b23b36c1dff0222cf 100644 --- a/src/libs/nav2_util/include/nav2_util/sensors/amcl_sensor.h +++ b/src/libs/nav2_util/include/nav2_util/sensors/sensor.h @@ -34,29 +34,29 @@ namespace amcl { // Forward declarations -class AMCLSensorData; +class SensorData; // Base class for all AMCL sensors -class AMCLSensor +class Sensor { // Default constructor - public: AMCLSensor(); + public: Sensor(); // Default destructor - public: virtual ~AMCLSensor(); + public: virtual ~Sensor(); // Update the filter based on the action model. Returns true if the filter // has been updated. - public: virtual bool UpdateAction(pf_t *pf, AMCLSensorData *data); + public: virtual bool UpdateAction(pf_t *pf, SensorData *data); // Initialize the filter based on the sensor model. Returns true if the // filter has been initialized. - public: virtual bool InitSensor(pf_t *pf, AMCLSensorData *data); + public: virtual bool InitSensor(pf_t *pf, SensorData *data); // Update the filter based on the sensor model. Returns true if the // filter has been updated. - public: virtual bool UpdateSensor(pf_t *pf, AMCLSensorData *data); + public: virtual bool UpdateSensor(pf_t *pf, SensorData *data); // Flag is true if this is the action sensor public: bool is_action; @@ -75,18 +75,18 @@ class AMCLSensor public: virtual void ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig); // Draw sensor data - public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data); + public: virtual void UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, SensorData *data); #endif }; // Base class for all AMCL sensor measurements -class AMCLSensorData +class SensorData { // Pointer to sensor that generated the data - public: AMCLSensor *sensor; - virtual ~AMCLSensorData() {} + public: Sensor *sensor; + virtual ~SensorData() {} // Data timestamp public: double time; diff --git a/src/libs/nav2_util/src/sensors/laser.cpp b/src/libs/nav2_util/src/sensors/laser.cpp index b75651ee3f7779aece833812fa8f3bf4e9f47b01..c45bc097311c9a49c62df3b2e0f4cd3beb7285d6 100644 --- a/src/libs/nav2_util/src/sensors/laser.cpp +++ b/src/libs/nav2_util/src/sensors/laser.cpp @@ -39,7 +39,7 @@ using namespace amcl; //////////////////////////////////////////////////////////////////////////////// // Default constructor -Laser::Laser(size_t max_beams, map_t* map) : AMCLSensor(), +Laser::Laser(size_t max_beams, map_t* map) : Sensor(), max_samples(0), max_obs(0), temp_obs(NULL) { @@ -118,7 +118,7 @@ Laser::SetModelLikelihoodFieldProb(double z_hit, //////////////////////////////////////////////////////////////////////////////// // Apply the laser sensor model -bool Laser::UpdateSensor(pf_t *pf, AMCLSensorData *data) +bool Laser::UpdateSensor(pf_t *pf, SensorData *data) { if (this->max_beams < 2) return false; diff --git a/src/libs/nav2_util/src/sensors/odom.cpp b/src/libs/nav2_util/src/sensors/odom.cpp index e014efa803b27cc63847b8299a83293a98c94ceb..1d8cd354403b7680c9ffdb9406826b68387b7720 100644 --- a/src/libs/nav2_util/src/sensors/odom.cpp +++ b/src/libs/nav2_util/src/sensors/odom.cpp @@ -59,7 +59,7 @@ angle_diff(double a, double b) //////////////////////////////////////////////////////////////////////////////// // Default constructor -Odom::Odom() : AMCLSensor() +Odom::Odom() : Sensor() { this->time = 0.0; } @@ -110,7 +110,7 @@ Odom::SetModel( odom_model_t type, //////////////////////////////////////////////////////////////////////////////// // Apply the action model -bool Odom::UpdateAction(pf_t *pf, AMCLSensorData *data) +bool Odom::UpdateAction(pf_t *pf, SensorData *data) { OdomData *ndata; ndata = (OdomData*) data; diff --git a/src/libs/nav2_util/src/sensors/amcl_sensor.cpp b/src/libs/nav2_util/src/sensors/sensor.cpp similarity index 79% rename from src/libs/nav2_util/src/sensors/amcl_sensor.cpp rename to src/libs/nav2_util/src/sensors/sensor.cpp index b5d8234414b3f822a4dfb4f376259c27c39d103d..4f9fc3fb369d1155e3d5a45d99a80a18c71c5651 100644 --- a/src/libs/nav2_util/src/sensors/amcl_sensor.cpp +++ b/src/libs/nav2_util/src/sensors/sensor.cpp @@ -28,24 +28,24 @@ /////////////////////////////////////////////////////////////////////////// -#include "nav2_util/sensors/amcl_sensor.h" +#include "nav2_util/sensors/sensor.h" using namespace amcl; //////////////////////////////////////////////////////////////////////////////// // Default constructor -AMCLSensor::AMCLSensor() +Sensor::Sensor() { return; } -AMCLSensor::~AMCLSensor() +Sensor::~Sensor() { } //////////////////////////////////////////////////////////////////////////////// // Apply the action model -bool AMCLSensor::UpdateAction(pf_t * /*pf*/, AMCLSensorData * /*data*/) +bool Sensor::UpdateAction(pf_t * /*pf*/, SensorData * /*data*/) { return false; } @@ -53,7 +53,7 @@ bool AMCLSensor::UpdateAction(pf_t * /*pf*/, AMCLSensorData * /*data*/) //////////////////////////////////////////////////////////////////////////////// // Initialize the filter -bool AMCLSensor::InitSensor(pf_t * /*pf*/, AMCLSensorData * /*data*/) +bool Sensor::InitSensor(pf_t * /*pf*/, SensorData * /*data*/) { return false; } @@ -61,7 +61,7 @@ bool AMCLSensor::InitSensor(pf_t * /*pf*/, AMCLSensorData * /*data*/) //////////////////////////////////////////////////////////////////////////////// // Apply the sensor model -bool AMCLSensor::UpdateSensor(pf_t * /*pf*/, AMCLSensorData * /*data*/) +bool Sensor::UpdateSensor(pf_t * /*pf*/, SensorData * /*data*/) { return false; } @@ -71,7 +71,7 @@ bool AMCLSensor::UpdateSensor(pf_t * /*pf*/, AMCLSensorData * /*data*/) //////////////////////////////////////////////////////////////////////////////// // Setup the GUI -void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) +void Sensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) { return; } @@ -79,7 +79,7 @@ void AMCLSensor::SetupGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) //////////////////////////////////////////////////////////////////////////////// // Shutdown the GUI -void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) +void Sensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) { return; } @@ -87,7 +87,7 @@ void AMCLSensor::ShutdownGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig) //////////////////////////////////////////////////////////////////////////////// // Draw sensor data -void AMCLSensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, AMCLSensorData *data) +void Sensor::UpdateGUI(rtk_canvas_t *canvas, rtk_fig_t *robot_fig, SensorData *data) { return; } diff --git a/src/localization/nav2_amcl/src/amcl_node.cpp b/src/localization/nav2_amcl/src/amcl_node.cpp index 6909cd212e55fa8afd3a06d07b8f48f133920188..64939d0880f438d937a785a05dcccfc155ce6659 100644 --- a/src/localization/nav2_amcl/src/amcl_node.cpp +++ b/src/localization/nav2_amcl/src/amcl_node.cpp @@ -63,7 +63,7 @@ using amcl::ODOM_MODEL_OMNI; using amcl::ODOM_MODEL_DIFF_CORRECTED; using amcl::OdomData; using amcl::ODOM_MODEL_OMNI_CORRECTED; -using amcl::AMCLSensorData; +using amcl::SensorData; using amcl::LaserData; static double @@ -1055,7 +1055,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) odata.delta = delta; // Use the action data to update the filter - odom_->UpdateAction(pf_, reinterpret_cast<AMCLSensorData *>(&odata)); + odom_->UpdateAction(pf_, reinterpret_cast<SensorData *>(&odata)); // Pose at last filter update // this->pf_odom_pose = pose; @@ -1129,7 +1129,7 @@ AmclNode::laserReceived(sensor_msgs::msg::LaserScan::ConstSharedPtr laser_scan) (i * angle_increment); } - lasers_[laser_index]->UpdateSensor(pf_, reinterpret_cast<AMCLSensorData *>(&ldata)); + lasers_[laser_index]->UpdateSensor(pf_, reinterpret_cast<SensorData *>(&ldata)); lasers_update_[laser_index] = false;