Skip to content

Commit 18ec7eb

Browse files
ethanmusserethanmusser
authored
Merge pull request #50 from KodlabPenn/lcm-publisher
Lcm publisher
2 parents b09237c + 059568c commit 18ec7eb

File tree

9 files changed

+224
-63
lines changed

9 files changed

+224
-63
lines changed

‎examples/imu_example.cpp‎

Lines changed: 14 additions & 14 deletions
Original file line numberDiff line numberDiff line change
@@ -64,30 +64,30 @@ class AttitudeExample : public kodlab::mjbots::MjbotsControlLoop<IMULog,
6464
}
6565

6666
/**
67-
* @brief Prepares log
67+
* @brief Populates log message with data from current control loop cycle
6868
*/
6969
void PrepareLog() override
7070
{
7171
// IMUData Quaternion
72-
log_data_.attitude_quaternion[0] = imu_->get_quat().x();
73-
log_data_.attitude_quaternion[1] = imu_->get_quat().y();
74-
log_data_.attitude_quaternion[2] = imu_->get_quat().z();
75-
log_data_.attitude_quaternion[3] = imu_->get_quat().w();
72+
log_data_->attitude_quaternion[0] = imu_->get_quat().x();
73+
log_data_->attitude_quaternion[1] = imu_->get_quat().y();
74+
log_data_->attitude_quaternion[2] = imu_->get_quat().z();
75+
log_data_->attitude_quaternion[3] = imu_->get_quat().w();
7676

7777
// IMUData Euler Angles
78-
log_data_.attitude_euler[0] = imu_->get_euler().roll();
79-
log_data_.attitude_euler[1] = imu_->get_euler().pitch();
80-
log_data_.attitude_euler[2] = imu_->get_euler().yaw();
78+
log_data_->attitude_euler[0] = imu_->get_euler().roll();
79+
log_data_->attitude_euler[1] = imu_->get_euler().pitch();
80+
log_data_->attitude_euler[2] = imu_->get_euler().yaw();
8181

8282
// Angular Velocity
83-
log_data_.angular_velocity[0] = imu_->get_ang_rate().x();
84-
log_data_.angular_velocity[1] = imu_->get_ang_rate().y();
85-
log_data_.angular_velocity[2] = imu_->get_ang_rate().z();
83+
log_data_->angular_velocity[0] = imu_->get_ang_rate().x();
84+
log_data_->angular_velocity[1] = imu_->get_ang_rate().y();
85+
log_data_->angular_velocity[2] = imu_->get_ang_rate().z();
8686

8787
// Linear Acceleration
88-
log_data_.linear_acceleration[0] = imu_->get_accel().x();
89-
log_data_.linear_acceleration[1] = imu_->get_accel().y();
90-
log_data_.linear_acceleration[2] = imu_->get_accel().z();
88+
log_data_->linear_acceleration[0] = imu_->get_accel().x();
89+
log_data_->linear_acceleration[1] = imu_->get_accel().y();
90+
log_data_->linear_acceleration[2] = imu_->get_accel().z();
9191
}
9292

9393
};

‎examples/leg_2DoF_example.cpp‎

Lines changed: 12 additions & 11 deletions
Original file line numberDiff line numberDiff line change
@@ -85,19 +85,20 @@ class Hopping : public kodlab::mjbots::MjbotsControlLoop<LegLog, LegGains> {
8585
}
8686

8787
void PrepareLog() override {
88+
// Populate log message with data from current control loop cycle
8889
for (int servo = 0; servo < 2; servo++) {
89-
log_data_.positions[servo] = robot_->GetJointPositions()[servo];
90-
log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
91-
log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
92-
log_data_.torque_cmd[servo] = robot_->GetJointTorqueCmd()[servo];
90+
log_data_->positions[servo] = robot_->GetJointPositions()[servo];
91+
log_data_->velocities[servo] = robot_->GetJointVelocities()[servo];
92+
log_data_->modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
93+
log_data_->torque_cmd[servo] = robot_->GetJointTorqueCmd()[servo];
9394
}
94-
log_data_.limb_position[0] = z_ - z0_;
95-
log_data_.limb_position[1] = x_;
96-
log_data_.limb_vel[0] = d_z_;
97-
log_data_.limb_vel[1] = d_x_;
98-
log_data_.limb_wrench[0] = f_z_;
99-
log_data_.limb_wrench[1] = f_x_;
100-
log_data_.hybrid_mode = mode_;
95+
log_data_->limb_position[0] = z_ - z0_;
96+
log_data_->limb_position[1] = x_;
97+
log_data_->limb_vel[0] = d_z_;
98+
log_data_->limb_vel[1] = d_x_;
99+
log_data_->limb_wrench[0] = f_z_;
100+
log_data_->limb_wrench[1] = f_x_;
101+
log_data_->hybrid_mode = mode_;
101102
}
102103

103104
void ProcessInput() override {

‎examples/leg_3DoF_example.cpp‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -42,17 +42,20 @@ class Joints3DoF : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
4242
}
4343

4444
void PrepareLog() override {
45+
// Populate log message with data from current control loop cycle
4546
for (int servo = 0; servo < num_joints_; servo++) {
46-
log_data_.positions[servo] = robot_->GetJointPositions()[servo];
47-
log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
48-
log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
49-
log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
47+
log_data_->positions[servo] = robot_->GetJointPositions()[servo];
48+
log_data_->velocities[servo] = robot_->GetJointVelocities()[servo];
49+
log_data_->modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
50+
log_data_->torques[servo] = robot_->GetJointTorqueCmd()[servo];
5051
}
52+
53+
// Fill remaining log message fields with zeros
5154
for (int servo = num_joints_; servo < 13; servo++) {
52-
log_data_.positions[servo] = 0;
53-
log_data_.velocities[servo] = 0;
54-
log_data_.modes[servo] = 0;
55-
log_data_.torques[servo] = 0;
55+
log_data_->positions[servo] = 0;
56+
log_data_->velocities[servo] = 0;
57+
log_data_->modes[servo] = 0;
58+
log_data_->torques[servo] = 0;
5659
}
5760
}
5861
};

‎examples/proprio_example.cpp‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -60,17 +60,20 @@ class ProprioJoints : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
6060
}
6161

6262
void PrepareLog() override {
63+
// Populate log message with data from current control loop cycle
6364
for (int servo = 0; servo < num_joints_; servo++) {
64-
log_data_.positions[servo] = robot_->GetJointPositions()[servo];
65-
log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
66-
log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
67-
log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
65+
log_data_->positions[servo] = robot_->GetJointPositions()[servo];
66+
log_data_->velocities[servo] = robot_->GetJointVelocities()[servo];
67+
log_data_->modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
68+
log_data_->torques[servo] = robot_->GetJointTorqueCmd()[servo];
6869
}
70+
71+
// Fill remaining log message fields with zeros
6972
for (int servo = num_joints_; servo < 13; servo++) {
70-
log_data_.positions[servo] = 0;
71-
log_data_.velocities[servo] = 0;
72-
log_data_.modes[servo] = 0;
73-
log_data_.torques[servo] = 0;
73+
log_data_->positions[servo] = 0;
74+
log_data_->velocities[servo] = 0;
75+
log_data_->modes[servo] = 0;
76+
log_data_->torques[servo] = 0;
7477
}
7578
}
7679
};

‎examples/robot_example.cpp‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -29,19 +29,22 @@ class SimpleRobotControlLoop : public kodlab::mjbots::MjbotsControlLoop<ManyMoto
2929
}
3030
void PrepareLog() override
3131
{
32+
// Populate log message with data from current control loop cycle
3233
for (int servo = 0; servo < num_joints_; servo++)
3334
{
34-
log_data_.positions[servo] = robot_->GetJointPositions()[servo];
35-
log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
36-
log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
37-
log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
35+
log_data_->positions[servo] = robot_->GetJointPositions()[servo];
36+
log_data_->velocities[servo] = robot_->GetJointVelocities()[servo];
37+
log_data_->modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
38+
log_data_->torques[servo] = robot_->GetJointTorqueCmd()[servo];
3839
}
40+
41+
// Fill remaining log message fields with zeros
3942
for (int servo = num_joints_; servo < 13; servo++)
4043
{
41-
log_data_.positions[servo] = 0;
42-
log_data_.velocities[servo] = 0;
43-
log_data_.modes[servo] = 0;
44-
log_data_.torques[servo] = 0;
44+
log_data_->positions[servo] = 0;
45+
log_data_->velocities[servo] = 0;
46+
log_data_->modes[servo] = 0;
47+
log_data_->torques[servo] = 0;
4548
}
4649
}
4750

‎examples/spin_joints_example.cpp‎

Lines changed: 11 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -24,17 +24,20 @@ class Spin_Joint : public kodlab::mjbots::MjbotsControlLoop<ManyMotorLog> {
2424
}
2525

2626
void PrepareLog() override {
27+
// Populate log message with data from current control loop cycle
2728
for (int servo = 0; servo < num_joints_; servo++) {
28-
log_data_.positions[servo] = robot_->GetJointPositions()[servo];
29-
log_data_.velocities[servo] = robot_->GetJointVelocities()[servo];
30-
log_data_.modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
31-
log_data_.torques[servo] = robot_->GetJointTorqueCmd()[servo];
29+
log_data_->positions[servo] = robot_->GetJointPositions()[servo];
30+
log_data_->velocities[servo] = robot_->GetJointVelocities()[servo];
31+
log_data_->modes[servo] = static_cast<int>(mjbots_interface_->GetJointModes()[servo]);
32+
log_data_->torques[servo] = robot_->GetJointTorqueCmd()[servo];
3233
}
34+
35+
// Fill remaining log message fields with zeros
3336
for (int servo = num_joints_; servo < 13; servo++) {
34-
log_data_.positions[servo] = 0;
35-
log_data_.velocities[servo] = 0;
36-
log_data_.modes[servo] = 0;
37-
log_data_.torques[servo] = 0;
37+
log_data_->positions[servo] = 0;
38+
log_data_->velocities[servo] = 0;
39+
log_data_->modes[servo] = 0;
40+
log_data_->torques[servo] = 0;
3841
}
3942
}
4043
};

‎include/kodlab_mjbots_sdk/lcm_publisher.h‎

Lines changed: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
/**
2+
* @file lcm_publisher.h
3+
* @author Ethan J. Musser ([email protected])
4+
* @brief Provides `LcmPublisher` class which handles storage and publishing of
5+
* LCM messages.
6+
* @date 8/7/22
7+
*
8+
* @copyright (c) Copyright 2022 The Trustees of the University of Pennsylvania.
9+
* All rights reserved.
10+
*
11+
*/
12+
13+
#pragma once
14+
15+
#include <memory>
16+
#include <string>
17+
18+
#include "lcm/lcm-cpp.hpp"
19+
20+
namespace kodlab {
21+
22+
/**
23+
* @brief LCM message data container and publisher
24+
* @note One way to use the `lcm::LCM` object is to have a single object which
25+
* publishes multiple messages on multiple channels. To support this usage, the
26+
* internal `lcm::LCM` object is stored as a shared pointer, which is set during
27+
* construction and accessible later on. this pointer can be passed around and
28+
* used in multiple `LcmPublisher` objects.
29+
* @tparam Message LCM message type
30+
*/
31+
template<class Message>
32+
class LcmPublisher {
33+
34+
public:
35+
36+
/**
37+
* @brief Construct an LCM publisher object without an `lcm::LCM` object or
38+
* channel.
39+
*/
40+
LcmPublisher() = default;
41+
42+
/**
43+
* @brief Construct an LCM publisher object with a shared LCM object.
44+
* @param lcm `lcm::LCM` object shared pointer
45+
* @param channel channel name for publishing
46+
*/
47+
LcmPublisher(std::shared_ptr<lcm::LCM> lcm, const std::string &channel)
48+
: lcm_(lcm), channel_(channel) {}
49+
50+
/**
51+
* @brief Construct an LCM publisher object with an owned LCM object.
52+
* @param lcm `lcm::LCM` object rvalue (must transfer ownership)
53+
* @param channel channel name for publishing
54+
*/
55+
LcmPublisher(lcm::LCM &&lcm, const std::string &channel)
56+
: LcmPublisher(std::make_shared<lcm::LCM>(std::move(lcm)), channel) {}
57+
58+
/**
59+
* @brief Publish the data stored in `message_` on LCM channel `channel_`.
60+
*/
61+
void Publish() {
62+
lcm_->publish<Message>(channel_, message_.get());
63+
}
64+
65+
/**
66+
* @brief Retrieve shared pointer to the LCM object used for publishing.
67+
* @return shared pointer to `lcm::LCM` object
68+
*/
69+
[[nodiscard]] std::shared_ptr<lcm::LCM> get_lcm() const { return lcm_; }
70+
71+
/**
72+
* @brief Set the object used for publishing as a new, shared LCM object.
73+
* @param lcm shared pointer to `lcm::LCM` object
74+
*/
75+
void set_lcm(std::shared_ptr<lcm::LCM> lcm) { lcm_ = lcm; }
76+
77+
/**
78+
* @brief Set the object used for publishing as a new, owned LCM object.
79+
* @param lcm `lcm::LCM` object rvalue (must transfer ownership)
80+
*/
81+
void set_lcm(lcm::LCM &&lcm) {
82+
lcm_ = std::make_shared<lcm::LCM>(std::move(lcm));
83+
}
84+
85+
/**
86+
* @brief Retrieve the LCM publishing channel name.
87+
* @return name of LCM channel used for publishing
88+
*/
89+
[[nodiscard]] const std::string get_channel() const { return channel_; }
90+
91+
/**
92+
* @brief Set the LCM channel used for publishing.
93+
* @param channel name of LCM channel to be used for publishing
94+
*/
95+
void set_channel(const std::string &channel) { channel_ = channel; }
96+
97+
/**
98+
* @brief Retrieve a shared pointer to the internally stored message data.
99+
* @return shared pointer to internal message data
100+
*/
101+
[[nodiscard]] const std::shared_ptr<Message> get_message_shared_ptr() const {
102+
return message_;
103+
}
104+
105+
/**
106+
* @brief Retrieve a raw pointer to the internally stored message data.
107+
* @return raw pointer to internal message data
108+
*/
109+
[[nodiscard]] const Message *get_message_ptr() const {
110+
return message_.get();
111+
}
112+
113+
/**
114+
* @brief Retrieve read-only version of internally stored message data.
115+
* @return read-only copy of internal message data
116+
*/
117+
[[nodiscard]] const Message get_message_data() const { return *message_; }
118+
119+
private:
120+
121+
/**
122+
* @brief LCM object used for publishing.
123+
* @note This object can be shared by multiple `LcmPublishers`.
124+
*/
125+
std::shared_ptr<lcm::LCM> lcm_;
126+
127+
/**
128+
* @brief LCM publishing channel name.
129+
*/
130+
std::string channel_;
131+
132+
/**
133+
* @brief Internal message that is published on calls to `Publish`.
134+
*/
135+
std::shared_ptr<Message> message_ = std::make_shared<Message>();
136+
137+
};
138+
139+
} // kodlab

‎include/kodlab_mjbots_sdk/mjbots_control_loop.h‎

Lines changed: 14 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,7 @@
1111
#include "kodlab_mjbots_sdk/robot_base.h"
1212
#include "kodlab_mjbots_sdk/mjbots_hardware_interface.h"
1313
#include "kodlab_mjbots_sdk/lcm_subscriber.h"
14+
#include "kodlab_mjbots_sdk/lcm_publisher.h"
1415
#include "lcm/lcm-cpp.hpp"
1516
#include "real_time_tools/timer.hpp"
1617
#include "real_time_tools/hard_spinner.hpp"
@@ -145,8 +146,9 @@ class MjbotsControlLoop : public AbstractRealtimeObject {
145146
bool logging_ = false; /// Boolean to determine if logging is in use
146147
bool input_ = false; /// Boolean to determine if input is in use
147148
std::string logging_channel_name_; /// Channel name to publish logs to, leave empty if not publishing
148-
lcm::LCM lcm_; /// LCM object
149-
LogClass log_data_; /// object containing log data
149+
std::shared_ptr<lcm::LCM> lcm_; /// LCM object shared pointer
150+
LcmPublisher<LogClass> log_pub_; /// log LCM publisher
151+
std::shared_ptr<LogClass> log_data_; /// LCM log message data shared pointer
150152
LcmSubscriber<InputClass> lcm_sub_; /// LCM subscriber object
151153
float time_now_ = 0; /// Time since start in micro seconds
152154
};
@@ -160,6 +162,9 @@ MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::vect
160162
template<class log_type, class input_type, class robot_type>
161163
MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::vector<std::shared_ptr<kodlab::mjbots::JointMoteus>> joint_ptrs, const ControlLoopOptions &options)
162164
: AbstractRealtimeObject(options.realtime_params.main_rtp, options.realtime_params.can_cpu),
165+
lcm_(std::make_shared<lcm::LCM>()),
166+
log_pub_(lcm_, options.log_channel_name),
167+
log_data_(log_pub_.get_message_shared_ptr()),
163168
lcm_sub_(options.realtime_params.lcm_rtp, options.realtime_params.lcm_cpu, options.input_channel_name)
164169
{
165170
robot_ = std::make_shared<robot_type>( joint_ptrs,
@@ -177,6 +182,9 @@ MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::vect
177182
template<class log_type, class input_type, class robot_type>
178183
MjbotsControlLoop<log_type, input_type, robot_type>::MjbotsControlLoop(std::shared_ptr<robot_type>robot_in, const ControlLoopOptions &options)
179184
: AbstractRealtimeObject(options.realtime_params.main_rtp, options.realtime_params.can_cpu),
185+
lcm_(std::make_shared<lcm::LCM>()),
186+
log_pub_(lcm_, options.log_channel_name),
187+
log_data_(log_pub_.get_message_shared_ptr()),
180188
lcm_sub_(options.realtime_params.lcm_rtp, options.realtime_params.lcm_cpu, options.input_channel_name) {
181189
// Create robot object
182190
robot_ = robot_in;
@@ -214,16 +222,16 @@ void MjbotsControlLoop<log_type, input_type, robot_type>::SetupOptions(const Con
214222
template<class log_type, class input_type, class robot_type>
215223
void MjbotsControlLoop<log_type, input_type, robot_type>::AddTimingLog(float t, float margin, float message_duration) {
216224
if (logging_) {
217-
log_data_.timestamp = t;
218-
log_data_.margin = margin;
219-
log_data_.message_duration = message_duration;
225+
log_data_->timestamp = t;
226+
log_data_->margin = margin;
227+
log_data_->message_duration = message_duration;
220228
}
221229
}
222230

223231
template<class log_type, class input_type, class robot_type>
224232
void MjbotsControlLoop<log_type, input_type, robot_type>::PublishLog() {
225233
if (logging_)
226-
lcm_.publish(logging_channel_name_, &log_data_);
234+
log_pub_.Publish();
227235
}
228236

229237
template<class log_type, class input_type, class robot_type>

0 commit comments

Comments
 (0)