Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[refactor] Add base for pub sub class #22

Open
wants to merge 7 commits into
base: main
Choose a base branch
from
Open

[refactor] Add base for pub sub class #22

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
8f59f95
without template
Mar 13, 2025
6bde51c
refactor sub
Mar 13, 2025
20d86a2
fix
Mar 13, 2025
7ac1bc6
fix
Mar 13, 2025
b5508c2
fix
Mar 13, 2025
f0be71c
imu refactor
Mar 13, 2025
8509a7e
touch refactor
Mar 13, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 0 additions & 4 deletions src/module/mujoco_sim_module/mujoco_sim_module.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,11 +14,7 @@
#include "mujoco_sim_module/subscriber/joint_actuator_subscriber.h"

#ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2
#include "mujoco_sim_module/publisher/imu_sensor_ros2_publisher.h"
#include "mujoco_sim_module/publisher/joint_sensor_ros2_publisher.h"
#include "mujoco_sim_module/publisher/touch_sensor_ros2_publisher.h"
#include "mujoco_sim_module/subscriber/joint_actuator_ros2_subscriber.h"

#endif

namespace YAML {
Expand Down
73 changes: 56 additions & 17 deletions src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,8 +5,8 @@

namespace YAML {
template <>
struct convert<aimrt_mujoco_sim::mujoco_sim_module::publisher::ImuSensorPublisher::Options> {
using Options = aimrt_mujoco_sim::mujoco_sim_module::publisher::ImuSensorPublisher::Options;
struct convert<aimrt_mujoco_sim::mujoco_sim_module::publisher::ImuSensorPublisherBase::Options> {
using Options = aimrt_mujoco_sim::mujoco_sim_module::publisher::ImuSensorPublisherBase::Options;

static Node encode(const Options& rhs) {
Node node;
Expand Down Expand Up @@ -40,20 +40,34 @@ struct convert<aimrt_mujoco_sim::mujoco_sim_module::publisher::ImuSensorPublishe
} // namespace YAML

namespace aimrt_mujoco_sim::mujoco_sim_module::publisher {
void ImuSensorPublisherBase::SetMj(mjModel* m, mjData* d) {
m_ = m;
d_ = d;
}

void ImuSensorPublisher::Initialize(YAML::Node options_node) {
void ImuSensorPublisherBase::RegisterSensorAddr() {
sensor_addr_group_.framequat_addr = GetSensorAddr(m_, options_.bind_framequat),
sensor_addr_group_.gyro_addr = GetSensorAddr(m_, options_.bind_gyro),
sensor_addr_group_.accelerometer_addr = GetSensorAddr(m_, options_.bind_accelerometer);
}

void ImuSensorPublisherBase::CopySensorData(int addr, auto& dest, size_t n) {
if (addr >= 0) {
std::memcpy(&dest, &d_->sensordata[addr], n * sizeof(double));
}
}

void ImuSensorPublisherBase::InitializeBase(YAML::Node options_node) {
if (options_node && !options_node.IsNull())
options_ = options_node.as<Options>();

avg_interval_base_ = GetAvgIntervalBase(channel_frq_);

RegisterSensorAddr();

options_node = options_;
}

bool ret = aimrt::channel::RegisterPublishType<aimrt::protocols::sensor::ImuState>(publisher_);

AIMRT_CHECK_ERROR_THROW(ret, "Register publish type failed.");
void ImuSensorPublisher::Initialize(YAML::Node options_node) {
InitializeBase(options_node);
AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType<aimrt::protocols::sensor::ImuState>(publisher_), "Register publish type failed.");
}

void ImuSensorPublisher::PublishSensorData() {
Expand Down Expand Up @@ -106,16 +120,41 @@ void ImuSensorPublisher::PublishSensorData() {
}
}

void ImuSensorPublisher::RegisterSensorAddr() {
sensor_addr_group_.framequat_addr = GetSensorAddr(m_, options_.bind_framequat),
sensor_addr_group_.gyro_addr = GetSensorAddr(m_, options_.bind_gyro),
sensor_addr_group_.accelerometer_addr = GetSensorAddr(m_, options_.bind_accelerometer);
#ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2
void ImuSensorRos2Publisher::Initialize(YAML::Node options_node) {
InitializeBase(options_node);
AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType<sensor_msgs::msg::Imu>(publisher_), "Register publish type failed.");
}

void ImuSensorPublisher::CopySensorData(int addr, auto& dest, size_t n) {
if (addr >= 0) {
std::memcpy(&dest, &d_->sensordata[addr], n * sizeof(double));
void ImuSensorRos2Publisher::PublishSensorData() {
static constexpr uint32_t ONE_MB = 1024 * 1024;
if (counter_++ < avg_interval_) return;
auto state_array = std::make_unique<SensorStateGroup>();

CopySensorData(sensor_addr_group_.framequat_addr, state_array->orientation, 4);
CopySensorData(sensor_addr_group_.gyro_addr, state_array->angular_velocity, 3);
CopySensorData(sensor_addr_group_.accelerometer_addr, state_array->linear_acceleration, 3);

executor_.Execute([this, state_array = std::move(state_array)]() {
sensor_msgs::msg::Imu state;
state.header.stamp = rclcpp::Clock().now();
state.orientation.x = state_array->orientation.x;
state.orientation.y = state_array->orientation.y;
state.orientation.z = state_array->orientation.z;
state.orientation.w = state_array->orientation.w;
state.angular_velocity.x = state_array->angular_velocity.x;
state.angular_velocity.y = state_array->angular_velocity.y;
state.angular_velocity.z = state_array->angular_velocity.z;
state.linear_acceleration.x = state_array->linear_acceleration.x;
state.linear_acceleration.y = state_array->linear_acceleration.y;
state.linear_acceleration.z = state_array->linear_acceleration.z;
aimrt::channel::Publish(publisher_, state);
});
avg_interval_ += avg_interval_base_;
if (counter_ > ONE_MB) {
avg_interval_ -= ONE_MB;
counter_ -= ONE_MB;
}
}

#endif
} // namespace aimrt_mujoco_sim::mujoco_sim_module::publisher
82 changes: 48 additions & 34 deletions src/module/mujoco_sim_module/publisher/imu_sensor_publisher.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -10,9 +10,14 @@
#include "mujoco_sim_module/publisher/publisher_base.h"
#include "mujoco_sim_module/publisher/utils.h"

#ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2
#include <sensor_msgs/msg/imu.hpp>
#include "aimrt_module_ros2_interface/channel/ros2_channel.h"
#endif

namespace aimrt_mujoco_sim::mujoco_sim_module::publisher {

class ImuSensorPublisher : public PublisherBase {
class ImuSensorPublisherBase : public PublisherBase {
public:
struct Options {
std::string bind_site;
Expand All @@ -22,59 +27,48 @@ class ImuSensorPublisher : public PublisherBase {
};

public:
ImuSensorPublisher() = default;
~ImuSensorPublisher() override = default;
ImuSensorPublisherBase() = default;
~ImuSensorPublisherBase() override = default;

virtual void Initialize(YAML::Node options_node) = 0;
virtual std::string_view Type() const noexcept override = 0;
virtual void PublishSensorData() override = 0;

void Initialize(YAML::Node options_node) override;
void Start() override {}
void Shutdown() override {}

[[nodiscard]] std::string_view Type() const noexcept override { return "imu_sensor"; }
void SetPublisherHandle(aimrt::channel::PublisherRef publisher_handle) override { publisher_ = publisher_handle; }

void SetPublisherHandle(aimrt::channel::PublisherRef publisher_handle) override {
publisher_ = publisher_handle;
}
void SetExecutor(aimrt::executor::ExecutorRef executor) override { executor_ = executor; };

void SetMj(mjModel* m, mjData* d) override {
m_ = m;
d_ = d;
}
void SetFreq(uint32_t freq) override { channel_frq_ = freq; };

void SetExecutor(aimrt::executor::ExecutorRef executor) override {
executor_ = executor;
};
void SetMj(mjModel* m, mjData* d) override;

void SetFreq(uint32_t freq) override {
channel_frq_ = freq;
};

void PublishSensorData() override;

private:
protected:
void InitializeBase(YAML::Node options_node);
void RegisterSensorAddr();
void CopySensorData(int addr, auto& dest, size_t n);

private:
protected:
struct SensorAddrGroup {
int32_t framequat_addr = -1;
int32_t gyro_addr = -1;
int32_t accelerometer_addr = -1;
int32_t framequat_addr;
int32_t gyro_addr;
int32_t accelerometer_addr;
};

struct SensorStateGroup {
struct {
double w = 0.0, x = 0.0, y = 0.0, z = 0.0;
double w, x, y, z;
} orientation;
struct {
double x = 0.0, y = 0.0, z = 0.0;
double x, y, z;
} angular_velocity;
struct {
double x = 0.0, y = 0.0, z = 0.0;
double x, y, z;
} linear_acceleration;
};

void CopySensorData(int addr, auto& dest, size_t n);

private:
Options options_;

mjModel* m_ = nullptr;
Expand All @@ -86,10 +80,30 @@ class ImuSensorPublisher : public PublisherBase {
uint32_t channel_frq_ = 1000;
double avg_interval_base_ = 1.0;
double avg_interval_ = 0;

size_t imu_num_ = 0;
uint32_t counter_ = 0;

SensorAddrGroup sensor_addr_group_;
};

} // namespace aimrt_mujoco_sim::mujoco_sim_module::publisher
class ImuSensorPublisher : public ImuSensorPublisherBase {
public:
ImuSensorPublisher() = default;
~ImuSensorPublisher() override = default;

void Initialize(YAML::Node options_node) override;
std::string_view Type() const noexcept override { return "imu_sensor"; }
void PublishSensorData() override;
};
#ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2
class ImuSensorRos2Publisher : public ImuSensorPublisherBase {
public:
ImuSensorRos2Publisher() = default;
~ImuSensorRos2Publisher() override = default;

void Initialize(YAML::Node options_node) override;
std::string_view Type() const noexcept override { return "imu_sensor_ros2"; }
void PublishSensorData() override;
};
#endif
} // namespace aimrt_mujoco_sim::mujoco_sim_module::publisher
89 changes: 0 additions & 89 deletions src/module/mujoco_sim_module/publisher/imu_sensor_ros2_publisher.cc
View file
Open in desktop

This file was deleted.

Loading