Skip to content

Commit

Permalink
Renaming odometry_2d (#87)
Browse files Browse the repository at this point in the history
  • Loading branch information
ayrton04 committed Aug 14, 2019
1 parent c049bd4 commit 0cef924
Show file tree
Hide file tree
Showing 8 changed files with 48 additions and 63 deletions.
4 changes: 2 additions & 2 deletions fuse_models/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -95,8 +95,8 @@ add_library(
src/acceleration_2d.cpp
src/imu_2d.cpp
src/pose_2d/model.cpp
src/odometry_2d/model.cpp
src/odometry_2d/publisher.cpp
src/odometry_2d.cpp
src/odometry_2d_publisher.cpp
src/twist_2d/model.cpp
src/unicycle_2d/state_kinematic_constraint.cpp
src/unicycle_2d/ignition.cpp
Expand Down
4 changes: 2 additions & 2 deletions fuse_models/fuse_plugins.xml
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,7 @@
</description>
</class>

<class type="fuse_models::odometry_2d::Publisher" base_class_type="fuse_core::Publisher">
<class type="fuse_models::Odometry2DPublisher" base_class_type="fuse_core::Publisher">
<description>
Publisher plugin that publishes a nav_msgs::Odometry message and broadcasts a tf transform for optimized 2D
state data (combination of Position2DStamped, Orientation2DStamped, VelocityLinear2DStamped, and
Expand All @@ -26,7 +26,7 @@
acceleration constraints from IMU sensor data published by another node
</description>
</class>
<class type="fuse_models::odometry_2d::Model" base_class_type="fuse_core::SensorModel">
<class type="fuse_models::Odometry2D" base_class_type="fuse_core::SensorModel">
<description>
An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data
published by another node
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,10 +31,10 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef FUSE_MODELS_ODOMETRY_2D_MODEL_H
#define FUSE_MODELS_ODOMETRY_2D_MODEL_H
#ifndef FUSE_MODELS_ODOMETRY_2D_H
#define FUSE_MODELS_ODOMETRY_2D_H

#include <fuse_models/parameters/odometry_2d_model_params.h>
#include <fuse_models/parameters/odometry_2d_params.h>

#include <fuse_core/async_sensor_model.h>
#include <fuse_core/uuid.h>
Expand All @@ -48,9 +48,6 @@
namespace fuse_models
{

namespace odometry_2d
{

/**
* @brief An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data
* published by another node
Expand All @@ -62,7 +59,7 @@ namespace odometry_2d
* 2. Creates 2D velocity variables and constraints.
*
* This sensor really just separates out the pose and twist components of the message, and processes them just like the
* pose_2d::Model and twist_2d::Model classes.
* pose_2d::Odometry2D and twist_2d::Odometry2D classes.
*
* Parameters:
* - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to publish
Expand All @@ -79,21 +76,21 @@ namespace odometry_2d
* Subscribes:
* - \p topic (nav_msgs::Odometry) Odometry information at a given timestep
*/
class Model : public fuse_core::AsyncSensorModel
class Odometry2D : public fuse_core::AsyncSensorModel
{
public:
SMART_PTR_DEFINITIONS(Model);
using ParameterType = parameters::Odometry2DModelParams;
SMART_PTR_DEFINITIONS(Odometry2D);
using ParameterType = parameters::Odometry2DParams;

/**
* @brief Default constructor
*/
Model();
Odometry2D();

/**
* @brief Destructor
*/
virtual ~Model() = default;
virtual ~Odometry2D() = default;

/**
* @brief Callback for pose messages
Expand Down Expand Up @@ -134,8 +131,6 @@ class Model : public fuse_core::AsyncSensorModel
ros::Subscriber subscriber_;
};

} // namespace odometry_2d

} // namespace fuse_models

#endif // FUSE_MODELS_ODOMETRY_2D_MODEL_H
#endif // FUSE_MODELS_ODOMETRY_2D_H
Original file line number Diff line number Diff line change
Expand Up @@ -55,11 +55,8 @@
namespace fuse_models
{

namespace odometry_2d
{

/**
* @class Publisher plugin that publishes a nav_msgs::Odometry message and broadcasts a tf transform for optimized 2D
* @class Odometry2DPublisher plugin that publishes a nav_msgs::Odometry message and broadcasts a tf transform for optimized 2D
* state data (combination of Position2DStamped, Orientation2DStamped, VelocityLinear2DStamped, and
* VelocityAngular2DStamped).
*
Expand Down Expand Up @@ -92,21 +89,21 @@ namespace odometry_2d
* - tf, tf_static (tf2_msgs::TFMessage) Subscribes to tf data to obtain the requisite odom->base_link transform,
* but only if the world_frame_id is set to the value of the map_frame_id.
*/
class Publisher : public fuse_core::AsyncPublisher
class Odometry2DPublisher : public fuse_core::AsyncPublisher
{
public:
SMART_PTR_DEFINITIONS(Publisher);
SMART_PTR_DEFINITIONS(Odometry2DPublisher);
using ParameterType = parameters::Odometry2DPublisherParams;

/**
* @brief Constructor
*/
Publisher();
Odometry2DPublisher();

/**
* @brief Destructor
*/
virtual ~Publisher() = default;
virtual ~Odometry2DPublisher() = default;

protected:
/**
Expand Down Expand Up @@ -192,8 +189,6 @@ class Publisher : public fuse_core::AsyncPublisher
ros::Timer tf_publish_timer_;
};

} // namespace odometry_2d

} // namespace fuse_models

#endif // FUSE_MODELS_ODOMETRY_2D_PUBLISHER_H
Original file line number Diff line number Diff line change
Expand Up @@ -31,8 +31,8 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef FUSE_MODELS_PARAMETERS_ODOMETRY_2D_MODEL_PARAMS_H
#define FUSE_MODELS_PARAMETERS_ODOMETRY_2D_MODEL_PARAMS_H
#ifndef FUSE_MODELS_PARAMETERS_ODOMETRY_2D_PARAMS_H
#define FUSE_MODELS_PARAMETERS_ODOMETRY_2D_PARAMS_H

#include <fuse_models/parameters/parameter_base.h>

Expand All @@ -53,9 +53,9 @@ namespace parameters
{

/**
* @brief Defines the set of parameters required by the odometry_2d::Model class
* @brief Defines the set of parameters required by the Odometry2D class
*/
struct Odometry2DModelParams : public ParameterBase
struct Odometry2DParams : public ParameterBase
{
public:
/**
Expand Down Expand Up @@ -94,4 +94,4 @@ struct Odometry2DModelParams : public ParameterBase

} // namespace fuse_models

#endif // FUSE_MODELS_PARAMETERS_ODOMETRY_2D_MODEL_PARAMS_H
#endif // FUSE_MODELS_PARAMETERS_ODOMETRY_2D_PARAMS_H
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@ namespace parameters
{

/**
* @brief Defines the set of parameters required by the pose_2d::Model class
* @brief Defines the set of parameters required by the Odometry2DPublisher class
*/
struct Odometry2DPublisherParams : public ParameterBase
{
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,7 @@
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <fuse_models/common/sensor_proc.h>
#include <fuse_models/odometry_2d/model.h>
#include <fuse_models/odometry_2d.h>

#include <fuse_core/transaction.h>
#include <fuse_core/uuid.h>
Expand All @@ -45,22 +45,19 @@


// Register this sensor model with ROS as a plugin.
PLUGINLIB_EXPORT_CLASS(fuse_models::odometry_2d::Model, fuse_core::SensorModel)
PLUGINLIB_EXPORT_CLASS(fuse_models::Odometry2D, fuse_core::SensorModel)

namespace fuse_models
{

namespace odometry_2d
{

Model::Model() :
Odometry2D::Odometry2D() :
fuse_core::AsyncSensorModel(1),
device_id_(fuse_core::uuid::NIL),
tf_listener_(tf_buffer_)
{
}

void Model::onInit()
void Odometry2D::onInit()
{
// Read settings from the parameter sever
device_id_ = fuse_variables::loadDeviceId(private_node_handle_);
Expand All @@ -77,24 +74,25 @@ void Model::onInit()
}
}

void Model::onStart()
void Odometry2D::onStart()
{
if (!params_.position_indices.empty() ||
!params_.orientation_indices.empty() ||
!params_.linear_velocity_indices.empty() ||
!params_.angular_velocity_indices.empty())
{
previous_pose_.reset();
subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, &Model::process, this);
subscriber_ =
node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, &Odometry2D::process, this);
}
}

void Model::onStop()
void Odometry2D::onStop()
{
subscriber_.shutdown();
}

void Model::process(const nav_msgs::Odometry::ConstPtr& msg)
void Odometry2D::process(const nav_msgs::Odometry::ConstPtr& msg)
{
// Create a transaction object
auto transaction = fuse_core::Transaction::make_shared();
Expand Down Expand Up @@ -151,6 +149,4 @@ void Model::process(const nav_msgs::Odometry::ConstPtr& msg)
sendTransaction(transaction);
}

} // namespace odometry_2d

} // namespace fuse_models
Original file line number Diff line number Diff line change
Expand Up @@ -31,7 +31,7 @@
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*/
#include <fuse_models/odometry_2d/publisher.h>
#include <fuse_models/odometry_2d_publisher.h>
#include <fuse_models/unicycle_2d/predict.h>

#include <fuse_core/async_publisher.h>
Expand All @@ -50,22 +50,19 @@
#include <vector>

// Register this publisher with ROS as a plugin.
PLUGINLIB_EXPORT_CLASS(fuse_models::odometry_2d::Publisher, fuse_core::Publisher)
PLUGINLIB_EXPORT_CLASS(fuse_models::Odometry2DPublisher, fuse_core::Publisher)

namespace fuse_models
{

namespace odometry_2d
{

Publisher::Publisher() :
Odometry2DPublisher::Odometry2DPublisher() :
fuse_core::AsyncPublisher(1),
device_id_(fuse_core::uuid::NIL),
latest_stamp_(Synchronizer::TIME_ZERO)
{
}

void Publisher::onInit()
void Odometry2DPublisher::onInit()
{
// Read settings from the parameter sever
device_id_ = fuse_variables::loadDeviceId(private_node_handle_);
Expand All @@ -81,10 +78,14 @@ void Publisher::onInit()
odom_pub_ = node_handle_.advertise<nav_msgs::Odometry>(ros::names::resolve(params_.topic), params_.queue_size);

tf_publish_timer_ = node_handle_.createTimer(
ros::Duration(1.0 / params_.tf_publish_frequency), &Publisher::tfPublishTimerCallback, this, false, false);
ros::Duration(1.0 / params_.tf_publish_frequency),
&Odometry2DPublisher::tfPublishTimerCallback,
this,
false,
false);
}

void Publisher::notifyCallback(
void Odometry2DPublisher::notifyCallback(
fuse_core::Transaction::ConstSharedPtr transaction,
fuse_core::Graph::ConstSharedPtr graph)
{
Expand Down Expand Up @@ -166,20 +167,20 @@ void Publisher::notifyCallback(
}
}

void Publisher::onStart()
void Odometry2DPublisher::onStart()
{
synchronizer_ = Synchronizer(device_id_);
latest_stamp_ = Synchronizer::TIME_ZERO;
odom_output_ = nav_msgs::Odometry();
tf_publish_timer_.start();
}

void Publisher::onStop()
void Odometry2DPublisher::onStop()
{
tf_publish_timer_.stop();
}

bool Publisher::getState(
bool Odometry2DPublisher::getState(
const fuse_core::Graph& graph,
const ros::Time& stamp,
const fuse_core::UUID& device_id,
Expand Down Expand Up @@ -232,7 +233,7 @@ bool Publisher::getState(
return true;
}

void Publisher::tfPublishTimerCallback(const ros::TimerEvent& event)
void Odometry2DPublisher::tfPublishTimerCallback(const ros::TimerEvent& event)
{
if (latest_stamp_ == Synchronizer::TIME_ZERO)
{
Expand Down Expand Up @@ -302,6 +303,4 @@ void Publisher::tfPublishTimerCallback(const ros::TimerEvent& event)
tf_broadcaster_.sendTransform(trans);
}

} // namespace odometry_2d

} // namespace fuse_models

0 comments on commit 0cef924

Please sign in to comment.