Skip to content

Refactor sensor handling and remove timestamp from message headers #55

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

Merged
mauricefallon merged 2 commits into from
Jun 20, 2026
Merged

Refactor sensor handling and remove timestamp from message headers #55

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cfb14b0
Remove timestamp setting from message headers in rosfrontend callbacks
lo-imperatore Jun 18, 2026
2210e82
Refactor sensor handling to use vector instead of unordered_set
lo-imperatore Jun 18, 2026
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
3 changes: 0 additions & 3 deletions pronto_ros/include/pronto_ros/ros_frontend.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -265,7 +265,6 @@ void ROSFrontEnd::initCallback(std::shared_ptr<MsgT> msg, const SensorId& sensor
}
if(initialized_list_.count(sensor_id) > 0 && !initialized_list_[sensor_id])
{
msg->header.set__stamp(nh_->get_clock()->now());
initialized_list_[sensor_id] = static_cast<SensingModule<MsgT>*>(init_modules_[sensor_id])->processMessageInit(
msg.get(),
initialized_list_,
Expand Down Expand Up @@ -311,7 +310,6 @@ void ROSFrontEnd::callback(std::shared_ptr<MsgT> msg, const SensorId& sensor_id)
// appropriate casting to the right type and call to the process message
// function to get the update
// Record start time
msg->header.set__stamp(nh_->get_clock()->now());
#if DEBUG_MODE
auto start = std::chrono::high_resolution_clock::now();
#endif
Expand Down Expand Up @@ -451,7 +449,6 @@ void ROSFrontEnd::callback(std::shared_ptr<MsgT> msg, const SensorId& sensor_id)
template <class PrimaryMsgT, class SecondaryMsgT>
void ROSFrontEnd::secondaryCallback(std::shared_ptr<SecondaryMsgT > msg, const SensorId& sensor_id)
{
msg->header.set__stamp(nh_->get_clock()->now());
auto a = dynamic_cast<DualSensingModule<PrimaryMsgT,SecondaryMsgT>*>(static_cast<SensingModule<PrimaryMsgT>*>(active_modules_[sensor_id]));
a->processSecondaryMessage(*msg);
}
Expand Down
16 changes: 11 additions & 5 deletions pronto_ros2_node/src/pronto_ros2_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,8 +17,11 @@
#include "pi3hat_moteus_int_msgs/msg/joints_states.hpp"
#include "sensor_msgs/msg/imu.hpp"

#include <algorithm>
#include <pinocchio/parsers/urdf.hpp>
#include <pinocchio/algorithm/kinematics.hpp>
#include <string>
#include <vector>
#include "pinocchio/algorithm/model.hpp"


Expand All @@ -36,7 +39,6 @@ namespace pronto
class Pronto_Ros2 : public rclcpp::Node
{
using SensorList = std::vector<std::string>;
using SensorSet = std::unordered_set<std::string>;
public:
Pronto_Ros2():
Node("Pronto_ROS2_Node")
Expand Down Expand Up @@ -69,13 +71,17 @@ namespace pronto
RCLCPP_INFO_STREAM(get_logger(),init_sensors_.size()<<" "<<active_sensors_.size());
for(auto &init:init_sensors_)
{
all_sensors_.insert(init);
if (std::find(all_sensors_.begin(), all_sensors_.end(), init) == all_sensors_.end()) {
all_sensors_.push_back(init);
}
RCLCPP_INFO_STREAM(get_logger(),"Init sensor "<<init.c_str());
}
for(auto &active:active_sensors_)
{
RCLCPP_INFO_STREAM(get_logger(),"Active sensor "<<active.c_str());
all_sensors_.insert(active);
if (std::find(all_sensors_.begin(), all_sensors_.end(), active) == all_sensors_.end()) {
all_sensors_.push_back(active);
}
}


Expand Down Expand Up @@ -165,7 +171,7 @@ namespace pronto
// create front end
ros_fe_ = std::make_shared<pronto::ROSFrontEnd>(this->shared_from_this(),true);

for (SensorSet::iterator it = all_sensors_.begin(); it != all_sensors_.end(); ++it)
for (SensorList::iterator it = all_sensors_.begin(); it != all_sensors_.end(); ++it)
{
RCLCPP_INFO(get_logger(),"allocate sensor %s",it->c_str());
declare_parameter<bool>(*it + ".roll_forward_on_receive",false);
Expand Down Expand Up @@ -341,7 +347,7 @@ namespace pronto
private:
SensorList init_sensors_;
SensorList active_sensors_;
SensorSet all_sensors_;
SensorList all_sensors_;
std::string urdf_file_;

//declare the mammal utils stuff and the ros_frontend
Expand Down
Loading