Skip to content

consolidated vehicle speed and turn angle to single float message type #10

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 1 commit into from
Jan 19, 2026
Merged

consolidated vehicle speed and turn angle to single float message type #10

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
16 changes: 7 additions & 9 deletions include/ap1/planning/planner_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,16 +22,14 @@

#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/planning/math_utils.hpp"
#include "ap1/planning/waypoint_utils.hpp"

using ap1_msgs::msg::SpeedProfileStamped;
using ap1_msgs::msg::TargetPathStamped;
using ap1_msgs::msg::TurnAngleStamped;
using ap1_msgs::msg::VehicleSpeedStamped;
using ap1_msgs::msg::FloatStamped;
using geometry_msgs::msg::Point;
using rclcpp::TimerBase;
using std_msgs::msg::Float32MultiArray;
Expand Down Expand Up @@ -71,20 +69,20 @@ class PlannerNode : public rclcpp::Node
// Subscriptions
// Note: use SharedPtrs for all messages since they're dynamically allocated in ROS
rclcpp::Subscription<Float32MultiArray>::SharedPtr hd_map_sub_; // WRONG TYPE SHOULD BE XML
rclcpp::Subscription<VehicleSpeedStamped>::SharedPtr vehicle_speed_sub_;
rclcpp::Subscription<FloatStamped>::SharedPtr vehicle_speed_sub_;
rclcpp::Subscription<Point>::SharedPtr target_location_sub_;
rclcpp::Subscription<VehicleSpeedStamped>::SharedPtr target_speed_sub_;
rclcpp::Subscription<FloatStamped>::SharedPtr target_speed_sub_;

// Publishers
rclcpp::Publisher<SpeedProfileStamped>::SharedPtr speed_profile_pub_;
rclcpp::Publisher<TargetPathStamped>::SharedPtr target_path_pub_;

// # Callbacks
void on_hd_map(const Float32MultiArray::SharedPtr); // WRONG TYPE SHOULD BE XML
void on_turn_angle(const TurnAngleStamped::SharedPtr);
void on_vehicle_speed(const VehicleSpeedStamped::SharedPtr);
void on_turn_angle(const FloatStamped::SharedPtr);
void on_vehicle_speed(const FloatStamped::SharedPtr);
void on_target_location(const geometry_msgs::msg::Point::SharedPtr loc);
void on_target_speed(const VehicleSpeedStamped::SharedPtr);
void on_target_speed(const FloatStamped::SharedPtr);

/**
* @brief Mocks map data for testing.
Expand Down
15 changes: 7 additions & 8 deletions src/planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,8 +24,7 @@

#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/planning/planner_node.hpp"

Expand All @@ -46,13 +45,13 @@ PlannerNode::PlannerNode(double rate_hz) : Node("planner_node"), rate_hz_(rate_h
hd_map_sub_ = this->create_subscription<Float32MultiArray>(
"/ap1/map/full_had_map", 10,
std::bind(&PlannerNode::on_hd_map, 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(&PlannerNode::on_vehicle_speed, this, std::placeholders::_1));
target_location_sub_ = this->create_subscription<Point>(
"/ap1/control/target_location", 10,
std::bind(&PlannerNode::on_target_location, this, std::placeholders::_1));
target_speed_sub_ = this->create_subscription<VehicleSpeedStamped>(
target_speed_sub_ = this->create_subscription<FloatStamped>(
"/ap1/control/target_speed", 10,
std::bind(&PlannerNode::on_target_speed, this, std::placeholders::_1));

Expand Down Expand Up @@ -80,19 +79,19 @@ void PlannerNode::on_hd_map(const Float32MultiArray::SharedPtr)
// todo: implement
}

void PlannerNode::on_turn_angle(const TurnAngleStamped::SharedPtr)
void PlannerNode::on_turn_angle(const FloatStamped::SharedPtr)
{
// todo: implement
}

void PlannerNode::on_vehicle_speed(const VehicleSpeedStamped::SharedPtr)
void PlannerNode::on_vehicle_speed(const FloatStamped::SharedPtr)
{
// todo: implement
}

void PlannerNode::on_target_speed(const VehicleSpeedStamped::SharedPtr msg)
void PlannerNode::on_target_speed(const FloatStamped::SharedPtr msg)
{
this->speed_ = msg->speed;
this->speed_ = msg->value;

std::string s = "Command: set speed to " + std::to_string(this->speed_);
RCLCPP_INFO_STREAM(this->get_logger(), s);
Expand Down