Skip to content

Messages #8

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
obaidmm merged 2 commits into from
Jan 19, 2026
Merged

Messages #8

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
89b246f
consolidated float messages into single type
cadence-m Dec 3, 2025
618c59d
Fixing message types in header
cadence-m Dec 4, 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
20 changes: 9 additions & 11 deletions include/ap1/control/control_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -11,11 +11,9 @@

#include "rclcpp/rclcpp.hpp"

#include "ap1_msgs/msg/motor_power_stamped.hpp"
#include "ap1_msgs/msg/speed_profile_stamped.hpp"
#include "ap1_msgs/msg/target_path_stamped.hpp"
#include "ap1_msgs/msg/turn_angle_stamped.hpp"
#include "ap1_msgs/msg/vehicle_speed_stamped.hpp"
#include "ap1_msgs/msg/float_stamped.hpp"

#include "ap1/control/ackermann_controller.hpp"
#include "ap1/control/icontroller.hpp"
Expand All @@ -40,24 +38,24 @@ class ControlNode : public rclcpp::Node
// stamped double or something
ap1_msgs::msg::SpeedProfileStamped speed_profile_;
ap1_msgs::msg::TargetPathStamped target_path_;
ap1_msgs::msg::VehicleSpeedStamped vehicle_speed_;
ap1_msgs::msg::TurnAngleStamped vehicle_turn_angle;
ap1_msgs::msg::FloatStamped vehicle_speed_;
ap1_msgs::msg::FloatStamped vehicle_turn_angle;

// Subs
rclcpp::Subscription<ap1_msgs::msg::TargetPathStamped>::SharedPtr target_path_sub_;
rclcpp::Subscription<ap1_msgs::msg::SpeedProfileStamped>::SharedPtr speed_profile_sub_;
rclcpp::Subscription<ap1_msgs::msg::VehicleSpeedStamped>::SharedPtr vehicle_speed_sub_;
rclcpp::Subscription<ap1_msgs::msg::TurnAngleStamped>::SharedPtr vehicle_turn_angle_sub_;
rclcpp::Subscription<ap1_msgs::msg::FloatStamped>::SharedPtr vehicle_speed_sub_;
rclcpp::Subscription<ap1_msgs::msg::FloatStamped>::SharedPtr vehicle_turn_angle_sub_;

// Pubs
rclcpp::Publisher<ap1_msgs::msg::TurnAngleStamped>::SharedPtr turning_angle_pub_;
rclcpp::Publisher<ap1_msgs::msg::MotorPowerStamped>::SharedPtr motor_power_pub_; // between -1 and 1? probably
rclcpp::Publisher<ap1_msgs::msg::FloatStamped>::SharedPtr turning_angle_pub_;
rclcpp::Publisher<ap1_msgs::msg::FloatStamped>::SharedPtr motor_power_pub_; // between -1 and 1? probably

// Methods
void on_speed_profile(const ap1_msgs::msg::SpeedProfileStamped speed_profile);
void on_path(const ap1_msgs::msg::TargetPathStamped target_path);
void on_speed(const ap1_msgs::msg::VehicleSpeedStamped speed);
void on_turn_angle(const ap1_msgs::msg::TurnAngleStamped turn_angle);
void on_speed(const ap1_msgs::msg::FloatStamped speed);
void on_turn_angle(const ap1_msgs::msg::FloatStamped turn_angle);
void control_loop_callback();

public:
Expand Down
30 changes: 14 additions & 16 deletions src/control_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,9 @@

#include "rclcpp/rclcpp.hpp"

#include "ap1_msgs/msg/motor_power_stamped.hpp"
#include "ap1_msgs/msg/speed_profile_stamped.hpp"
#include "ap1_msgs/msg/target_path_stamped.hpp"
#include "ap1_msgs/msg/turn_angle_stamped.hpp"
#include "ap1_msgs/msg/vehicle_speed_stamped.hpp"
#include "ap1_msgs/msg/float_stamped.hpp"

#include "ap1/control/control_node.hpp"
#include "ap1/control/icontroller.hpp"
Expand All @@ -33,20 +31,20 @@ void ControlNode::on_path(const TargetPathStamped target_path)
target_path_ = target_path;
}

void ControlNode::on_speed(const VehicleSpeedStamped speed)
void ControlNode::on_speed(const FloatStamped & msg)
{
vehicle_speed_ = speed;
vehicle_speed_ = msg.value;
}

void ControlNode::on_turn_angle(const TurnAngleStamped turn_angle)
void ControlNode::on_turn_angle(const FloatStamped & msg)
{
vehicle_turn_angle = turn_angle;
vehicle_turn_angle = msg.value;
}

void ControlNode::control_loop_callback()
{
// the car's current velocity. we only support moving forward atp
const vec3f velocity(this->vehicle_speed_.speed, 0, 0); // +x is always forward on the car
const vec3f velocity(this->vehicle_speed_, 0, 0); // +x is always forward on the car

const bool PATH_IS_STALE = false, SPEED_PROFILE_IS_STALE = false; // TEMP

Expand Down Expand Up @@ -77,16 +75,16 @@ void ControlNode::control_loop_callback()
RCLCPP_INFO(this->get_logger(), "CMD: {throttle: %.2f, steering: %.2f}", cmd.throttle, cmd.steering);

// pack the turn angle into a message
TurnAngleStamped turn_msg;
FloatStamped turn_msg;
turn_msg.header.stamp = this->now();
turn_msg.header.frame_id = "base_link";
turn_msg.angle = cmd.steering; // rads
turn_msg.value = cmd.steering; // rads

// pack the power into a message
MotorPowerStamped pwr_msg;
FloatStamped pwr_msg;
pwr_msg.header.stamp = this->now();
pwr_msg.header.frame_id = "base_link";
pwr_msg.power = cmd.throttle; // [-1, 1]
pwr_msg.value = cmd.throttle; // [-1, 1]
// pwr_msg.power = 1.0f;

// send both messages out
Expand All @@ -105,16 +103,16 @@ ControlNode::ControlNode(const std::string& cfg_path, float rate_hz)
target_path_sub_ = this->create_subscription<TargetPathStamped>(
"ap1/planning/target_path", 10,
std::bind(&ControlNode::on_path, this, std::placeholders::_1));
vehicle_speed_sub_ = this->create_subscription<VehicleSpeedStamped>(
vehicle_speed_sub_ = this->create_subscription<FloatStamped>(
"ap1/actuation/speed_actual", 10,
std::bind(&ControlNode::on_speed, this, std::placeholders::_1));
vehicle_turn_angle_sub_ = this->create_subscription<TurnAngleStamped>(
vehicle_turn_angle_sub_ = this->create_subscription<FloatStamped>(
"ap1/actuation/turn_angle_actual", 10,
std::bind(&ControlNode::on_turn_angle, this, std::placeholders::_1));

// Pubs
turning_angle_pub_ = this->create_publisher<TurnAngleStamped>("ap1/control/turn_angle", 10);
motor_power_pub_ = this->create_publisher<MotorPowerStamped>("ap1/control/motor_power", 10);
turning_angle_pub_ = this->create_publisher<FloatStamped>("ap1/control/turn_angle", 10);
motor_power_pub_ = this->create_publisher<FloatStamped>("ap1/control/motor_power", 10);

// Create Control Loop
timer_ = this->create_wall_timer(std::chrono::duration<double>(1.0 / rate_hz),
Expand Down