modelec/modelec-marcel-ROS
1
0
Fork 0
mirror of https://github.com/modelec/modelec-marcel-ROS.git synced 2026-01-18 16:47:43 +01:00
Code Issues Packages Projects Releases Wiki Activity

start pcb action

Browse Source
This commit is contained in:
acki
2025-05-12 11:01:51 -04:00
parent e893a7a1ce
commit 711cce8b41
31 changed files with 961 additions and 581 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
simulated_pcb/odo.py
View File

@@ -4,6 +4,7 @@ import math
import threading
import argparse
class SimulatedPCB:
def __init__(self, port='/dev/pts/6', baud=115200):
self.ser = serial.Serial(port, baud, timeout=1)

15
src/modelec_com/CMakeLists.txt
View File

@@ -18,7 +18,7 @@ find_package(modelec_interfaces REQUIRED)
find_package(modelec_utils REQUIRED)
find_library(WIRINGPI_LIB wiringPi)
# USB Arduino Listener Node
# Serial listener Node
add_executable(serial_listener src/multiple_serial_listener.cpp)
ament_target_dependencies(serial_listener rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(serial_listener Boost::system modelec_utils::utils modelec_utils::config)
@@ -27,7 +27,7 @@ target_include_directories(serial_listener PUBLIC
$<INSTALL_INTERFACE:include>
)
# USB Arduino Logic Processor Node
# PCB Odometry Node
add_executable(pcb_odo_interface src/pcb_odo_interface.cpp)
ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(pcb_odo_interface modelec_utils::utils modelec_utils::config)
@@ -36,7 +36,7 @@ target_include_directories(pcb_odo_interface PUBLIC
$<INSTALL_INTERFACE:include>
)
# USB Arduino Logic Processor Node
# PCB Alim Node
add_executable(pcb_alim_interface src/pcb_alim_interface.cpp)
ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(pcb_alim_interface modelec_utils::utils modelec_utils::config)
@@ -45,6 +45,15 @@ target_include_directories(pcb_alim_interface PUBLIC
$<INSTALL_INTERFACE:include>
)
# PCB Action Node
add_executable(pcb_action_interface src/pcb_action_interface.cpp)
ament_target_dependencies(pcb_action_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(pcb_action_interface modelec_utils::utils modelec_utils::config)
target_include_directories(pcb_action_interface PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# PCA9685 Listener Node
add_executable(pca9685_controller src/pca9685_controller.cpp)
ament_target_dependencies(pca9685_controller rclcpp std_msgs modelec_interfaces)

91
src/modelec_com/include/modelec_com/multiple_serial_listener.hpp
View File

@@ -12,64 +12,65 @@
namespace Modelec
{
class SerialListener {
private:
bool status_;
int bauds_;
std::string serial_port_;
int max_message_len_;
class SerialListener
{
private:
bool status_;
int bauds_;
std::string serial_port_;
int max_message_len_;
boost::asio::io_service& io_;
std::vector<char> read_buffer_;
std::deque<std::string> write_queue_;
std::mutex write_mutex_;
std::thread io_thread_;
boost::asio::io_service& io_;
std::vector<char> read_buffer_;
std::deque<std::string> write_queue_;
std::mutex write_mutex_;
std::thread io_thread_;
void start_async_read();
void start_async_write();
void start_async_read();
void start_async_write();
public:
std::string name_;
boost::asio::serial_port port_;
public:
std::string name_;
boost::asio::serial_port port_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
SerialListener(const std::string& name, int bauds, const std::string& serial_port,
int max_message_len, boost::asio::io_service& io);
SerialListener(const std::string& name, int bauds, const std::string& serial_port,
int max_message_len, boost::asio::io_service& io);
~SerialListener();
~SerialListener();
void close();
void close();
void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; }
bool IsOk() const { return status_; }
void SetMaxMessageLen(int max_message_len) { max_message_len_ = max_message_len; }
bool IsOk() const { return status_; }
void SetOk() { status_ = true; }
void SetOk() { status_ = true; }
void write(std_msgs::msg::String::SharedPtr msg);
};
void write(std_msgs::msg::String::SharedPtr msg);
};
class MultipleSerialListener : public rclcpp::Node
{
public:
MultipleSerialListener();
~MultipleSerialListener() override;
class MultipleSerialListener : public rclcpp::Node
{
public:
MultipleSerialListener();
~MultipleSerialListener() override;
private:
int default_max_message_len_ = MAX_MESSAGE_LEN;
private:
int default_max_message_len_ = MAX_MESSAGE_LEN;
std::map<std::string, std::shared_ptr<SerialListener>> serial_listeners;
std::map<std::string, std::shared_ptr<SerialListener>> serial_listeners;
boost::asio::io_service io;
boost::asio::io_service io;
rclcpp::Service<modelec_interfaces::srv::AddSerialListener>::SharedPtr add_serial_listener_service_;
void add_serial_listener(
const std::shared_ptr<modelec_interfaces::srv::AddSerialListener::Request> request,
std::shared_ptr<modelec_interfaces::srv::AddSerialListener::Response> response);
rclcpp::Service<modelec_interfaces::srv::AddSerialListener>::SharedPtr add_serial_listener_service_;
void add_serial_listener(
const std::shared_ptr<modelec_interfaces::srv::AddSerialListener::Request> request,
std::shared_ptr<modelec_interfaces::srv::AddSerialListener::Response> response);
OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
void updateConnection();
};
} // namespace Modelec
OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter>& parameters);
void updateConnection();
};
} // namespace Modelec

12
src/modelec_com/include/modelec_com/pca9685_controller.hpp
View File

@@ -17,16 +17,18 @@
#define SERVO_MAX 492
#define PCA9685_ADDR 0x40 // I2C address for the PCA9685
namespace Modelec {
class PCA9685Controller : public rclcpp::Node {
namespace Modelec
{
class PCA9685Controller : public rclcpp::Node
{
public:
PCA9685Controller();
private:
int fd; // File descriptor for I2C communication
int fd; // File descriptor for I2C communication
rclcpp::Subscription<modelec_interfaces::msg::PCA9685Servo>::SharedPtr servo_subscriber_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr clear_subscriber_;
int frequency = 50; // Default PWM frequency (Hz)
int frequency = 50; // Default PWM frequency (Hz)
std::unordered_set<int> active_servos;
@@ -34,7 +36,7 @@ namespace Modelec {
rclcpp::Service<modelec_interfaces::srv::AddServoMotor>::SharedPtr add_servo_service_;
OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter> &parameters);
rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter>& parameters);
void updatePCA9685();
// PCA9685 configuration and control

33
src/modelec_com/include/modelec_com/pcb_action_interface.hpp Normal file
View File

@@ -0,0 +1,33 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
namespace Modelec
{
class PCBActionInterface : public rclcpp::Node
{
public:
PCBActionInterface();
rclcpp::CallbackGroup::SharedPtr pcb_callback_group_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
std::thread pcb_executor_thread_;
~PCBActionInterface() override;
private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr pcb_subscriber_;
void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
public:
void SendToPCB(const std::string& data) const;
void SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data = {}) const;
void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {}) const;
};
}

183
src/modelec_com/include/modelec_com/pcb_alim_interface.hpp
View File

@@ -2,6 +2,7 @@
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <modelec_interfaces/srv/alim_out.hpp>
#include <modelec_interfaces/srv/alim_in.hpp>
#include <modelec_interfaces/srv/alim_bau.hpp>
@@ -11,96 +12,96 @@
namespace Modelec
{
class PCBAlimInterface : public rclcpp::Node
{
public:
PCBAlimInterface();
rclcpp::CallbackGroup::SharedPtr pcb_callback_group_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
std::thread pcb_executor_thread_;
~PCBAlimInterface() override;
struct PCBData
class PCBAlimInterface : public rclcpp::Node
{
bool success;
int value;
public:
PCBAlimInterface();
rclcpp::CallbackGroup::SharedPtr pcb_callback_group_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
std::thread pcb_executor_thread_;
~PCBAlimInterface() override;
struct PCBData
{
bool success;
int value;
};
struct PCBBau
{
bool success;
bool activate;
};
private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr pcb_subscriber_;
void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
rclcpp::Subscription<modelec_interfaces::msg::AlimEmg>::SharedPtr pcb_emg_subscriber_;
void PCBEmgCallback(const modelec_interfaces::msg::AlimEmg::SharedPtr msg) const;
rclcpp::Service<modelec_interfaces::srv::AlimOut>::SharedPtr pcb_out_service_;
rclcpp::Service<modelec_interfaces::srv::AlimIn>::SharedPtr pcb_in_service_;
rclcpp::Service<modelec_interfaces::srv::AlimBau>::SharedPtr pcb_bau_service_;
rclcpp::Service<modelec_interfaces::srv::AlimEmg>::SharedPtr pcb_emg_service_;
rclcpp::Service<modelec_interfaces::srv::AlimTemp>::SharedPtr pcb_temp_service_;
std::queue<std::promise<PCBData>> pcb_out_promises_;
std::mutex pcb_out_mutex_;
std::queue<std::promise<PCBData>> pcb_in_promises_;
std::mutex pcb_in_mutex_;
std::queue<std::promise<PCBBau>> pcb_bau_promises_;
std::mutex pcb_bau_mutex_;
std::queue<std::promise<bool>> pcb_emg_promises_;
std::mutex pcb_emg_mutex_;
std::queue<std::promise<PCBData>> pcb_temp_promises_;
std::mutex pcb_temp_mutex_;
void HandleGetPCBOutData(
const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response);
void HandleSetPCBOutData(
const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response);
void HandleGetPCBInData(
const std::shared_ptr<modelec_interfaces::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimIn::Response> response);
void HandleGetPCBBauData(
const std::shared_ptr<modelec_interfaces::srv::AlimBau::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimBau::Response> response);
void HandleSetPCBEmgData(
const std::shared_ptr<modelec_interfaces::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimEmg::Response> response);
void HandleGetPCBTempData(
const std::shared_ptr<modelec_interfaces::srv::AlimTemp::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimTemp::Response> response);
void ResolveGetPCBOutRequest(const PCBData& value);
void ResolveSetPCBOutRequest(const PCBData& value);
void ResolveGetPCBInRequest(const PCBData& value);
void ResolveGetPCBBauRequest(const PCBBau& value);
void ResolveSetPCBEmgRequest(bool value);
void ResolveGetPCBTempRequest(const PCBData& value);
public:
void SendToPCB(const std::string& data) const;
void SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data = {}) const;
void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {}) const;
};
struct PCBBau
{
bool success;
bool activate;
};
private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr pcb_subscriber_;
void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
rclcpp::Subscription<modelec_interfaces::msg::AlimEmg>::SharedPtr pcb_emg_subscriber_;
void PCBEmgCallback(const modelec_interfaces::msg::AlimEmg::SharedPtr msg) const;
rclcpp::Service<modelec_interfaces::srv::AlimOut>::SharedPtr pcb_out_service_;
rclcpp::Service<modelec_interfaces::srv::AlimIn>::SharedPtr pcb_in_service_;
rclcpp::Service<modelec_interfaces::srv::AlimBau>::SharedPtr pcb_bau_service_;
rclcpp::Service<modelec_interfaces::srv::AlimEmg>::SharedPtr pcb_emg_service_;
rclcpp::Service<modelec_interfaces::srv::AlimTemp>::SharedPtr pcb_temp_service_;
std::queue<std::promise<PCBData>> pcb_out_promises_;
std::mutex pcb_out_mutex_;
std::queue<std::promise<PCBData>> pcb_in_promises_;
std::mutex pcb_in_mutex_;
std::queue<std::promise<PCBBau>> pcb_bau_promises_;
std::mutex pcb_bau_mutex_;
std::queue<std::promise<bool>> pcb_emg_promises_;
std::mutex pcb_emg_mutex_;
std::queue<std::promise<PCBData>> pcb_temp_promises_;
std::mutex pcb_temp_mutex_;
void HandleGetPCBOutData(
const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response);
void HandleSetPCBOutData(
const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response);
void HandleGetPCBInData(
const std::shared_ptr<modelec_interfaces::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimIn::Response> response);
void HandleGetPCBBauData(
const std::shared_ptr<modelec_interfaces::srv::AlimBau::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimBau::Response> response);
void HandleSetPCBEmgData(
const std::shared_ptr<modelec_interfaces::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimEmg::Response> response);
void HandleGetPCBTempData(
const std::shared_ptr<modelec_interfaces::srv::AlimTemp::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimTemp::Response> response);
void ResolveGetPCBOutRequest(const PCBData& value);
void ResolveSetPCBOutRequest(const PCBData& value);
void ResolveGetPCBInRequest(const PCBData& value);
void ResolveGetPCBBauRequest(const PCBBau& value);
void ResolveSetPCBEmgRequest(bool value);
void ResolveGetPCBTempRequest(const PCBData& value);
public:
void SendToPCB(const std::string &data) const;
void SendToPCB(const std::string& order, const std::string& elem, const std::vector<std::string>& data = {}) const;
void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendOrder(const std::string &elem, const std::vector<std::string> &data = {}) const;
};
} // namespace Modelec
} // namespace Modelec

264
src/modelec_com/include/modelec_com/pcb_odo_interface.hpp
View File

@@ -25,137 +25,139 @@
#include <modelec_interfaces/srv/odometry_set_pid.hpp>
#include <modelec_interfaces/srv/odometry_add_waypoint.hpp>
namespace Modelec {
class PCBOdoInterface : public rclcpp::Node {
public:
PCBOdoInterface();
rclcpp::CallbackGroup::SharedPtr pcb_callback_group_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
std::thread pcb_executor_thread_;
~PCBOdoInterface() override;
struct OdometryData {
long x;
long y;
double theta;
};
struct PIDData
namespace Modelec
{
class PCBOdoInterface : public rclcpp::Node
{
float p;
float i;
float d;
public:
PCBOdoInterface();
rclcpp::CallbackGroup::SharedPtr pcb_callback_group_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
std::thread pcb_executor_thread_;
~PCBOdoInterface() override;
struct OdometryData
{
long x;
long y;
double theta;
};
struct PIDData
{
float p;
float i;
float d;
};
private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr pcb_subscriber_;
void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_pos_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometrySpeed>::SharedPtr odo_speed_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryToF>::SharedPtr odo_tof_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_pid_publisher_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const;
void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const;
rclcpp::Service<modelec_interfaces::srv::OdometryToF>::SharedPtr get_tof_service_;
rclcpp::Service<modelec_interfaces::srv::OdometrySpeed>::SharedPtr get_speed_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryPosition>::SharedPtr get_position_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryStart>::SharedPtr set_start_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryGetPid>::SharedPtr get_pid_service_;
rclcpp::Service<modelec_interfaces::srv::OdometrySetPid>::SharedPtr set_pid_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryAddWaypoint>::SharedPtr add_waypoint_service_;
// Promises and mutexes to synchronize service responses asynchronously
std::queue<std::promise<long>> tof_promises_;
std::mutex tof_mutex_;
std::queue<std::promise<OdometryData>> speed_promises_;
std::mutex speed_mutex_;
std::queue<std::promise<OdometryData>> pos_promises_;
std::mutex pos_mutex_;
std::queue<std::promise<bool>> start_promises_;
std::mutex start_mutex_;
std::queue<std::promise<PIDData>> get_pid_promises_;
std::mutex get_pid_mutex_;
std::queue<std::promise<bool>> set_pid_promises_;
std::mutex set_pid_mutex_;
std::queue<std::promise<bool>> add_waypoint_promises_;
std::mutex add_waypoint_mutex_;
// Service handlers using async wait with promises
void HandleGetTof(const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response);
void HandleGetSpeed(const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response);
void HandleGetPosition(const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response);
void HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response);
void HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response);
void HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response);
void HandleAddWaypoint(const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response);
// Resolving methods called by subscriber callback
void ResolveToFRequest(long distance);
void ResolveSpeedRequest(const OdometryData& speed);
void ResolvePositionRequest(const OdometryData& position);
void ResolveStartRequest(bool start);
void ResolveGetPIDRequest(const PIDData& pid);
void ResolveSetPIDRequest(bool success);
void ResolveAddWaypointRequest(bool success);
int timeout_ms = 1000;
int attempt = 5;
public:
void SendToPCB(const std::string& data) const;
void SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data = {}) const;
void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {}) const;
void GetPos() const;
void GetSpeed() const;
void GetToF(const int& tof) const;
void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
void SetRobotPos(long x, long y, double theta) const;
void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const;
void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta) const;
void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const;
void SetStart(bool start) const;
void GetPID() const;
void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const;
void SetPID(float p, float i, float d) const;
};
private:
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pcb_publisher_;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr pcb_subscriber_;
void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_pos_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometrySpeed>::SharedPtr odo_speed_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryToF>::SharedPtr odo_tof_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_pid_publisher_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const;
void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const;
rclcpp::Service<modelec_interfaces::srv::OdometryToF>::SharedPtr get_tof_service_;
rclcpp::Service<modelec_interfaces::srv::OdometrySpeed>::SharedPtr get_speed_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryPosition>::SharedPtr get_position_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryStart>::SharedPtr set_start_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryGetPid>::SharedPtr get_pid_service_;
rclcpp::Service<modelec_interfaces::srv::OdometrySetPid>::SharedPtr set_pid_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryAddWaypoint>::SharedPtr add_waypoint_service_;
// Promises and mutexes to synchronize service responses asynchronously
std::queue<std::promise<long>> tof_promises_;
std::mutex tof_mutex_;
std::queue<std::promise<OdometryData>> speed_promises_;
std::mutex speed_mutex_;
std::queue<std::promise<OdometryData>> pos_promises_;
std::mutex pos_mutex_;
std::queue<std::promise<bool>> start_promises_;
std::mutex start_mutex_;
std::queue<std::promise<PIDData>> get_pid_promises_;
std::mutex get_pid_mutex_;
std::queue<std::promise<bool>> set_pid_promises_;
std::mutex set_pid_mutex_;
std::queue<std::promise<bool>> add_waypoint_promises_;
std::mutex add_waypoint_mutex_;
// Service handlers using async wait with promises
void HandleGetTof(const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response);
void HandleGetSpeed(const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response);
void HandleGetPosition(const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response);
void HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response);
void HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response);
void HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response);
void HandleAddWaypoint(const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response);
// Resolving methods called by subscriber callback
void ResolveToFRequest(long distance);
void ResolveSpeedRequest(const OdometryData& speed);
void ResolvePositionRequest(const OdometryData& position);
void ResolveStartRequest(bool start);
void ResolveGetPIDRequest(const PIDData& pid);
void ResolveSetPIDRequest(bool success);
void ResolveAddWaypointRequest(bool success);
int timeout_ms = 1000;
int attempt = 5;
public:
void SendToPCB(const std::string &data) const;
void SendToPCB(const std::string& order, const std::string& elem, const std::vector<std::string>& data = {}) const;
void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendOrder(const std::string &elem, const std::vector<std::string> &data = {}) const;
void GetPos() const;
void GetSpeed() const;
void GetToF(const int &tof) const;
void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
void SetRobotPos(long x, long y, double theta) const;
void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const;
void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta) const;
void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const;
void SetStart(bool start) const;
void GetPID() const;
void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const;
void SetPID(float p, float i, float d) const;
};
} // namespace Modelec

49
src/modelec_com/src/multiple_serial_listener.cpp
View File

@@ -27,10 +27,14 @@ namespace Modelec
read_buffer_.resize(max_message_len_);
start_async_read();
io_thread_ = std::thread([this]() {
try {
io_thread_ = std::thread([this]()
{
try
{
io_.run();
} catch (const std::exception& e) {
}
catch (const std::exception& e)
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "IO thread exception: %s", e.what());
}
});
@@ -46,7 +50,8 @@ namespace Modelec
void SerialListener::close()
{
if (status_) {
if (status_)
{
if (port_.is_open()) port_.close();
io_.stop();
if (io_thread_.joinable()) io_thread_.join();
@@ -60,8 +65,10 @@ namespace Modelec
port_.async_read_some(
boost::asio::buffer(read_buffer_),
[this](const boost::system::error_code& ec, std::size_t bytes_transferred) {
if (!ec && bytes_transferred > 0) {
[this](const boost::system::error_code& ec, std::size_t bytes_transferred)
{
if (!ec && bytes_transferred > 0)
{
auto msg = std_msgs::msg::String();
msg.data = std::string(read_buffer_.begin(), read_buffer_.begin() + bytes_transferred);
if (publisher_)
@@ -69,8 +76,10 @@ namespace Modelec
publisher_->publish(msg);
}
start_async_read(); // continue reading
} else {
start_async_read(); // continue reading
}
else
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async read error: %s", ec.message().c_str());
}
});
@@ -82,7 +91,8 @@ namespace Modelec
bool write_in_progress = !write_queue_.empty();
write_queue_.push_back(msg->data);
if (!write_in_progress) {
if (!write_in_progress)
{
start_async_write();
}
}
@@ -94,14 +104,19 @@ namespace Modelec
boost::asio::async_write(
port_,
boost::asio::buffer(write_queue_.front()),
[this](const boost::system::error_code& ec, std::size_t /*length*/) {
[this](const boost::system::error_code& ec, std::size_t /*length*/)
{
std::lock_guard<std::mutex> lock(write_mutex_);
if (!ec) {
if (!ec)
{
write_queue_.pop_front();
if (!write_queue_.empty()) {
start_async_write(); // continue writing
if (!write_queue_.empty())
{
start_async_write(); // continue writing
}
} else {
}
else
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async write error: %s", ec.message().c_str());
}
});
@@ -157,7 +172,8 @@ namespace Modelec
response->publisher = listener->publisher_->get_topic_name();
response->subscriber = listener->subscriber_->get_topic_name();
RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Serial listener %s fully created", request->name.c_str());
RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Serial listener %s fully created",
request->name.c_str());
}
rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange(
@@ -168,7 +184,8 @@ namespace Modelec
for (const auto& parameter : parameters)
{
if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() == "max_message_len")
if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() ==
"max_message_len")
{
updateConnection();
}

86
src/modelec_com/src/pca9685_controller.cpp
View File

@@ -1,16 +1,19 @@
#include "modelec_com/pca9685_controller.hpp"
namespace Modelec {
PCA9685Controller::PCA9685Controller() : Node("pca9685_controller") {
if (wiringPiSetup() == -1) {
namespace Modelec
{
PCA9685Controller::PCA9685Controller() : Node("pca9685_controller")
{
if (wiringPiSetup() == -1)
{
RCLCPP_ERROR(this->get_logger(), "WiringPi setup failed.");
return;
}
// Initialize I2C communication with PCA9685
fd = wiringPiI2CSetup(PCA9685_ADDR);
if (fd == -1) {
if (fd == -1)
{
RCLCPP_ERROR(this->get_logger(), "Failed to initialize I2C communication with PCA9685.");
return;
}
@@ -19,8 +22,10 @@ namespace Modelec {
// Subscribe to topics for servo and solenoid control
servo_subscriber_ = this->create_subscription<modelec_interfaces::msg::PCA9685Servo>(
"servo_control", 10, [this](const modelec_interfaces::msg::PCA9685Servo::SharedPtr msg) {
if (active_servos.find(msg->pin) == active_servos.end()) {
"servo_control", 10, [this](const modelec_interfaces::msg::PCA9685Servo::SharedPtr msg)
{
if (active_servos.find(msg->pin) == active_servos.end())
{
RCLCPP_ERROR(this->get_logger(), "Servo on pin %d is not active.", msg->pin);
return;
}
@@ -28,17 +33,22 @@ namespace Modelec {
});
clear_subscriber_ = this->create_subscription<std_msgs::msg::Empty>(
"clear_pca9685", 10, [this](const std_msgs::msg::Empty::SharedPtr) {
"clear_pca9685", 10, [this](const std_msgs::msg::Empty::SharedPtr)
{
this->clearAllDevices();
});
add_servo_service_ = this->create_service<modelec_interfaces::srv::AddServoMotor>(
"add_servo", [this](const modelec_interfaces::srv::AddServoMotor::Request::SharedPtr request,
modelec_interfaces::srv::AddServoMotor::Response::SharedPtr response) {
if (active_servos.find(request->pin) == active_servos.end()) {
modelec_interfaces::srv::AddServoMotor::Response::SharedPtr response)
{
if (active_servos.find(request->pin) == active_servos.end())
{
active_servos.insert(request->pin);
response->success = true;
} else {
}
else
{
response->success = false;
}
});
@@ -50,7 +60,8 @@ namespace Modelec {
// Handle dynamic parameter changes
parameter_callback_handle_ = this->add_on_set_parameters_callback(
[this](const std::vector<rclcpp::Parameter> &parameters) {
[this](const std::vector<rclcpp::Parameter>& parameters)
{
return onParameterChange(parameters);
});
@@ -58,19 +69,22 @@ namespace Modelec {
}
// Initialize PCA9685 in normal operation mode
void PCA9685Controller::initializePCA9685() {
void PCA9685Controller::initializePCA9685()
{
writePCA9685Register(0x00, 0x00);
}
// Configure the PWM frequency
void PCA9685Controller::configurePCA9685Frequency(int frequency) {
void PCA9685Controller::configurePCA9685Frequency(int frequency)
{
int prescale = static_cast<int>(PCA9685_FREQUENCY / (PCA9685_RESOLUTION * frequency) - 1);
writePCA9685Register(0x00, 0x10); // Enter sleep mode
writePCA9685Register(0xFE, prescale); // Set prescale value
writePCA9685Register(0x00, 0x00); // Restart PCA9685
writePCA9685Register(0x00, 0x10); // Enter sleep mode
writePCA9685Register(0xFE, prescale); // Set prescale value
writePCA9685Register(0x00, 0x00); // Restart PCA9685
}
void PCA9685Controller::SetPCA9685PWM(int channel, int on_time, int off_time) {
void PCA9685Controller::SetPCA9685PWM(int channel, int on_time, int off_time)
{
wiringPiI2CWriteReg8(fd, 0x06 + 4 * channel, on_time & 0xFF);
wiringPiI2CWriteReg8(fd, 0x07 + 4 * channel, on_time >> 8);
wiringPiI2CWriteReg8(fd, 0x08 + 4 * channel, off_time & 0xFF);
@@ -78,32 +92,41 @@ namespace Modelec {
}
// Set the servo angle on a specific channel
void PCA9685Controller::setServoPWM(int channel, double angle) {
void PCA9685Controller::setServoPWM(int channel, double angle)
{
int on_time = static_cast<int>(SERVO_MIN + (SERVO_MAX - SERVO_MIN) * angle / 180);
SetPCA9685PWM(channel, 0, on_time);
}
// Clear all channels (reset devices)
void PCA9685Controller::clearAllDevices() {
for (int channel = 0; channel < 16; ++channel) {
void PCA9685Controller::clearAllDevices()
{
for (int channel = 0; channel < 16; ++channel)
{
SetPCA9685PWM(channel, 0, 0);
}
}
// Write to a specific PCA9685 register
void PCA9685Controller::writePCA9685Register(uint8_t reg, uint8_t value) {
if (wiringPiI2CWriteReg8(fd, reg, value) == -1) {
void PCA9685Controller::writePCA9685Register(uint8_t reg, uint8_t value)
{
if (wiringPiI2CWriteReg8(fd, reg, value) == -1)
{
RCLCPP_ERROR(this->get_logger(), "Failed to write to register 0x%02X", reg);
}
}
// Handle dynamic parameter changes
rcl_interfaces::msg::SetParametersResult PCA9685Controller::onParameterChange(const std::vector<rclcpp::Parameter> &parameters) {
rcl_interfaces::msg::SetParametersResult PCA9685Controller::onParameterChange(
const std::vector<rclcpp::Parameter>& parameters)
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto &parameter : parameters) {
if (parameter.get_name() == "frequency") {
for (const auto& parameter : parameters)
{
if (parameter.get_name() == "frequency")
{
updatePCA9685();
}
}
@@ -112,18 +135,21 @@ namespace Modelec {
}
// Update PCA9685 when frequency parameter changes
void PCA9685Controller::updatePCA9685() {
void PCA9685Controller::updatePCA9685()
{
int new_frequency = this->get_parameter("frequency").as_int();
if (new_frequency != frequency) {
if (new_frequency != frequency)
{
frequency = new_frequency;
configurePCA9685Frequency(frequency);
}
}
}
int main(int argc, char **argv) {
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::PCA9685Controller>());
rclcpp::shutdown();
return 0;
}
}

137
src/modelec_com/src/pcb_action_interface.cpp Normal file
View File

@@ -0,0 +1,137 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_com/pcb_action_interface.hpp>
#include <modelec_utils/config.hpp>
#include <modelec_utils/utils.hpp>
namespace Modelec
{
PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface")
{
// Service to create a new serial listener
std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
if (!Config::load(config_path))
{
RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str());
}
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = Config::get<std::string>("config.usb.pcb.pcb_action.name", "pcb_action");
request->bauds = Config::get<int>("config.usb.pcb.pcb_action.baudrate", 115200);
request->serial_port = Config::get<std::string>("config.usb.pcb.pcb_action.port", "/dev/ttyUSB0");
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (auto res = result.get())
{
if (res->success)
{
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;
options.callback_group = pcb_callback_group_;
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
res->publisher, 10,
[this](const std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
}, options);
pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
pcb_executor_thread_ = std::thread([this]()
{
pcb_executor_->spin();
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
}
PCBActionInterface::~PCBActionInterface()
{
pcb_executor_->cancel();
if (pcb_executor_thread_.joinable())
{
pcb_executor_thread_.join();
}
}
void PCBActionInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
{
RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str());
std::vector<std::string> tokens = split(msg->data, ';');
}
void PCBActionInterface::SendToPCB(const std::string& data) const
{
auto message = std_msgs::msg::String();
message.data = data;
pcb_publisher_->publish(message);
}
void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data) const
{
std::string command = order + ";" + elem;
for (const auto& d : data)
{
command += ";" + d;
}
command += "\n";
SendToPCB(command);
}
void PCBActionInterface::GetData(const std::string& elem, const std::vector<std::string>& data) const
{
SendToPCB("GET", elem, data);
}
void PCBActionInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data) const
{
SendToPCB("SET", elem, data);
}
}
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Modelec::PCBActionInterface>();
// Increase number of threads explicitly!
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 2 /* or more threads! */);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}

3
src/modelec_com/src/pcb_alim_interface.cpp
View File

@@ -1,6 +1,5 @@
#include <modelec_com/pcb_alim_interface.hpp>
#include <modelec_utils/utils.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
@@ -426,7 +425,7 @@ namespace Modelec
}
void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data) const
const std::vector<std::string>& data) const
{
std::string command = order + ";" + elem;
for (const auto& d : data)

81
src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -45,7 +45,8 @@ namespace Modelec
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
res->publisher, 10,
[this](const std_msgs::msg::String::SharedPtr msg) {
[this](const std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
},
options);
@@ -53,7 +54,8 @@ namespace Modelec
pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
pcb_executor_thread_ = std::thread([this]() {
pcb_executor_thread_ = std::thread([this]()
{
pcb_executor_->spin();
});
@@ -114,7 +116,7 @@ namespace Modelec
get_tof_service_ = create_service<modelec_interfaces::srv::OdometryToF>(
"odometry/tof",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
{
HandleGetTof(request, response);
});
@@ -122,7 +124,7 @@ namespace Modelec
get_speed_service_ = create_service<modelec_interfaces::srv::OdometrySpeed>(
"odometry/speed",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
{
HandleGetSpeed(request, response);
});
@@ -130,7 +132,7 @@ namespace Modelec
get_position_service_ = create_service<modelec_interfaces::srv::OdometryPosition>(
"odometry/get_position",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
{
HandleGetPosition(request, response);
});
@@ -138,7 +140,7 @@ namespace Modelec
set_start_service_ = create_service<modelec_interfaces::srv::OdometryStart>(
"odometry/start",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
{
HandleGetStart(request, response);
});
@@ -146,7 +148,7 @@ namespace Modelec
get_pid_service_ = create_service<modelec_interfaces::srv::OdometryGetPid>(
"odometry/get_pid",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
{
HandleGetPID(request, response);
});
@@ -154,7 +156,7 @@ namespace Modelec
set_pid_service_ = create_service<modelec_interfaces::srv::OdometrySetPid>(
"odometry/set_pid",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
{
HandleSetPID(request, response);
});
@@ -162,7 +164,7 @@ namespace Modelec
add_waypoint_service_ = create_service<modelec_interfaces::srv::OdometryAddWaypoint>(
"odometry/add_waypoint",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response)
{
HandleAddWaypoint(request, response);
});
@@ -171,7 +173,8 @@ namespace Modelec
PCBOdoInterface::~PCBOdoInterface()
{
pcb_executor_->cancel();
if (pcb_executor_thread_.joinable()) {
if (pcb_executor_thread_.joinable())
{
pcb_executor_thread_.join();
}
}
@@ -378,7 +381,7 @@ namespace Modelec
}
void PCBOdoInterface::HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
{
std::promise<bool> promise;
auto future = promise.get_future();
@@ -393,7 +396,7 @@ namespace Modelec
}
void PCBOdoInterface::HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
{
std::promise<PIDData> promise;
auto future = promise.get_future();
@@ -413,7 +416,7 @@ namespace Modelec
}
void PCBOdoInterface::HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
{
std::promise<bool> promise;
auto future = promise.get_future();
@@ -448,11 +451,14 @@ namespace Modelec
void PCBOdoInterface::ResolveToFRequest(const long distance)
{
std::lock_guard<std::mutex> lock(tof_mutex_);
if (!tof_promises_.empty()) {
if (!tof_promises_.empty())
{
auto promise = std::move(tof_promises_.front());
tof_promises_.pop();
promise.set_value(distance);
} else {
}
else
{
RCLCPP_DEBUG(get_logger(), "No pending ToF request to resolve.");
}
}
@@ -460,11 +466,14 @@ namespace Modelec
void PCBOdoInterface::ResolveSpeedRequest(const OdometryData& speed)
{
std::lock_guard<std::mutex> lock(speed_mutex_);
if (!speed_promises_.empty()) {
if (!speed_promises_.empty())
{
auto promise = std::move(speed_promises_.front());
speed_promises_.pop();
promise.set_value(speed);
} else {
}
else
{
RCLCPP_DEBUG(get_logger(), "No pending Speed request to resolve.");
}
}
@@ -472,11 +481,14 @@ namespace Modelec
void PCBOdoInterface::ResolvePositionRequest(const OdometryData& position)
{
std::lock_guard<std::mutex> lock(pos_mutex_);
if (!pos_promises_.empty()) {
if (!pos_promises_.empty())
{
auto promise = std::move(pos_promises_.front());
pos_promises_.pop();
promise.set_value(position);
} else {
}
else
{
RCLCPP_DEBUG(get_logger(), "No pending Position request to resolve.");
}
}
@@ -484,11 +496,14 @@ namespace Modelec
void PCBOdoInterface::ResolveStartRequest(bool start)
{
std::lock_guard<std::mutex> lock(start_mutex_);
if (!start_promises_.empty()) {
if (!start_promises_.empty())
{
auto promise = std::move(start_promises_.front());
start_promises_.pop();
promise.set_value(start);
} else {
}
else
{
RCLCPP_DEBUG(get_logger(), "No pending Start request to resolve.");
}
}
@@ -496,11 +511,14 @@ namespace Modelec
void PCBOdoInterface::ResolveGetPIDRequest(const PIDData& pid)
{
std::lock_guard<std::mutex> lock(get_pid_mutex_);
if (!get_pid_promises_.empty()) {
if (!get_pid_promises_.empty())
{
auto promise = std::move(get_pid_promises_.front());
get_pid_promises_.pop();
promise.set_value(pid);
} else {
}
else
{
RCLCPP_DEBUG(get_logger(), "No pending PID request to resolve.");
}
}
@@ -508,11 +526,14 @@ namespace Modelec
void PCBOdoInterface::ResolveSetPIDRequest(bool success)
{
std::lock_guard<std::mutex> lock(set_pid_mutex_);
if (!set_pid_promises_.empty()) {
if (!set_pid_promises_.empty())
{
auto promise = std::move(set_pid_promises_.front());
set_pid_promises_.pop();
promise.set_value(success);
} else {
}
else
{
RCLCPP_DEBUG(get_logger(), "No pending Set PID request to resolve.");
}
}
@@ -520,11 +541,14 @@ namespace Modelec
void PCBOdoInterface::ResolveAddWaypointRequest(bool success)
{
std::lock_guard<std::mutex> lock(add_waypoint_mutex_);
if (!add_waypoint_promises_.empty()) {
if (!add_waypoint_promises_.empty())
{
auto promise = std::move(add_waypoint_promises_.front());
add_waypoint_promises_.pop();
promise.set_value(success);
} else {
}
else
{
RCLCPP_DEBUG(get_logger(), "No pending Add Waypoint request to resolve.");
}
}
@@ -640,10 +664,9 @@ namespace Modelec
SendOrder("PID", data);
}
} // Modelec
int main(int argc, char **argv)
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Modelec::PCBOdoInterface>();

5
src/modelec_core/launch/test.modelec.launch.yml
View File

@@ -15,6 +15,11 @@ launch:
exec: 'pcb_alim_interface'
name: 'pcb_alim_interface'
- node:
pkg: 'modelec_com'
exec: 'pcb_action_interface'
name: 'pcb_action_interface'
- node:
pkg: 'modelec_gui'
exec: "modelec_gui"

5
src/modelec_core/launch/test.no_gui.modelec.launch.yml
View File

@@ -15,6 +15,11 @@ launch:
exec: 'pcb_alim_interface'
name: 'pcb_alim_interface'
- node:
pkg: 'modelec_com'
exec: 'pcb_action_interface'
name: 'pcb_action_interface'
- node:
pkg: 'modelec_core'
exec: 'speed_result'

5
src/modelec_core/launch/test.no_strat.modelec.launch.yml
View File

@@ -15,6 +15,11 @@ launch:
exec: 'pcb_alim_interface'
name: 'pcb_alim_interface'
- node:
pkg: 'modelec_com'
exec: 'pcb_action_interface'
name: 'pcb_action_interface'
- node:
pkg: 'modelec_gui'
exec: "modelec_gui"

1
src/modelec_gui/include/modelec_gui/pages/home_page.hpp
View File

@@ -44,6 +44,5 @@ namespace ModelecGUI
QPushButton* yellow_button_;
QSvgRenderer* renderer_;
};
}

24
src/modelec_gui/include/modelec_gui/pages/odo_page.hpp
View File

@@ -16,31 +16,32 @@
#include <modelec_interfaces/srv/odometry_set_pid.hpp>
namespace ModelecGUI {
namespace ModelecGUI
{
class OdoPage : public QWidget
{
Q_OBJECT
public:
OdoPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
OdoPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr);
~OdoPage() override;
rclcpp::Node::SharedPtr get_node() const { return node_; }
private:
QVBoxLayout *mainLayout_;
QVBoxLayout* mainLayout_;
QPushButton* startButton_;
QLineEdit *xBox_, *yBox_, *thetaBox_;
QHBoxLayout *posLayout_;
QHBoxLayout* posLayout_;
QPushButton *askPID_;
QPushButton* askPID_;
QDoubleSpinBox *pPIDBox_, *iPIDBox_, *dPIDBox_;
QHBoxLayout *pidLayout_;
QPushButton *setPID_;
QHBoxLayout* pidLayout_;
QPushButton* setPID_;
QPushButton *askSpeed_;
QPushButton* askSpeed_;
QLineEdit *xSpeedBox_, *ySpeedBox_, *thetaSpeedBox_;
QHBoxLayout *speedLayout_;
QHBoxLayout* speedLayout_;
rclcpp::Node::SharedPtr node_;
@@ -54,5 +55,4 @@ namespace ModelecGUI {
void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
};
} // namespace Modelec
} // namespace Modelec

5
src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp
View File

@@ -24,13 +24,14 @@
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/int64.hpp>
namespace ModelecGUI {
namespace ModelecGUI
{
class TestMapPage : public QWidget
{
Q_OBJECT
public:
TestMapPage(rclcpp::Node::SharedPtr node, QWidget *parent = nullptr);
TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent = nullptr);
rclcpp::Node::SharedPtr get_node() const { return node_; }

10
src/modelec_gui/src/pages/home_page.cpp
View File

@@ -6,14 +6,16 @@
namespace ModelecGUI
{
HomePage::HomePage(rclcpp::Node::SharedPtr node, QWidget* parent)
: QWidget(parent), node_(node),
renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this))
: QWidget(parent), node_(node),
renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this))
{
yellow_button_ = new QPushButton("Yellow", this);
blue_button_ = new QPushButton("Blue", this);
yellow_button_->setStyleSheet("background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;");
blue_button_->setStyleSheet("background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;");
yellow_button_->setStyleSheet(
"background-color: rgba(255, 255, 0, 128); border: none; color: black; font-size: 24px;");
blue_button_->setStyleSheet(
"background-color: rgba(0, 0, 255, 128); border: none; color: white; font-size: 24px;");
yellow_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);
blue_button_->setSizePolicy(QSizePolicy::Expanding, QSizePolicy::Expanding);

125
src/modelec_gui/src/pages/map_page.cpp
View File

@@ -8,7 +8,10 @@
namespace ModelecGUI
{
MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)), node_(node)
MapPage::MapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent),
renderer(new QSvgRenderer(
QString(":/img/playmat/2025_FINAL.svg"),
this)), node_(node)
{
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
@@ -46,14 +49,17 @@ namespace ModelecGUI
enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 100,
[this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
"odometry/add_waypoint", 100,
[this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
{
if (lastWapointWasEnd)
{
qpoints.clear();
lastWapointWasEnd = false;
qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_));
qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_,
height() - robotPos.y * ratioBetweenMapAndWidgetY_));
}
if (msg->is_end)
@@ -61,31 +67,38 @@ namespace ModelecGUI
lastWapointWasEnd = true;
}
qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_));
qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_,
height() - msg->y * ratioBetweenMapAndWidgetY_));
update();
});
});
odometry_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
robotPos = *msg;
update();
});
});
score_sub_ = node_->create_subscription<std_msgs::msg::Int64>("strat/score", 10,
[this](const std_msgs::msg::Int64::SharedPtr msg) {
score_+= msg->data;
score_label_->setText(QString("Score: %1").arg(score_));
});
[this](const std_msgs::msg::Int64::SharedPtr msg)
{
score_ += msg->data;
score_label_->setText(
QString("Score: %1").arg(score_));
});
obstacle_added_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/added", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
OnObstacleReceived(msg);
});
});
obstacle_removed_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/removed", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
obstacle_removed_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
"nav/obstacle/removed", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
obstacle_.erase(msg->id);
});
});
enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("enemy/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
@@ -97,24 +110,29 @@ namespace ModelecGUI
});
strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("/strat/start_time", 10,
[this](const std_msgs::msg::Int64::SharedPtr msg){
isGameStarted_ = true;
start_time_ = msg->data;
});
[this](
const std_msgs::msg::Int64::SharedPtr msg)
{
isGameStarted_ = true;
start_time_ = msg->data;
});
strat_state_sub_ = node_->create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
[this](const modelec_interfaces::msg::StratState::SharedPtr msg){
[this](const modelec_interfaces::msg::StratState::SharedPtr msg)
{
if (msg->state == modelec_interfaces::msg::StratState::STOP)
{
RCLCPP_INFO(node_->get_logger(), "Game stop");
isGameStarted_ = false;
}
});
});
// client to nav/map
map_client_ = node_->create_client<modelec_interfaces::srv::MapSize>("nav/map_size");
while (!map_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!map_client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
@@ -134,8 +152,10 @@ namespace ModelecGUI
}
ask_map_obstacle_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_map_obstacle");
while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
@@ -149,22 +169,24 @@ namespace ModelecGUI
void MapPage::AskMap()
{
reset_timer_ = node_->create_wall_timer(
std::chrono::seconds(1),
[this]() {
ask_map_obstacle_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_map_obstacle");
while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
std::chrono::seconds(1),
[this]()
{
ask_map_obstacle_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_map_obstacle");
while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(node_->get_logger(), "Waiting for the service...");
}
RCLCPP_INFO(node_->get_logger(), "Waiting for the service...");
}
ask_map_obstacle_client_->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
ask_map_obstacle_client_->async_send_request(std::make_shared<std_srvs::srv::Empty::Request>());
reset_timer_->cancel();
});
reset_timer_->cancel();
});
}
void MapPage::Reset()
@@ -202,7 +224,8 @@ namespace ModelecGUI
// --- Draw lines ---
painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide
for (size_t i = 0; i + 1 < qpoints.size(); ++i) {
for (size_t i = 0; i + 1 < qpoints.size(); ++i)
{
painter.drawLine(qpoints[i], qpoints[i + 1]);
}
@@ -211,11 +234,12 @@ namespace ModelecGUI
// --- Draw colored points ---
const int radius = 5;
for (size_t i = 0; i < qpoints.size(); ++i) {
for (size_t i = 0; i < qpoints.size(); ++i)
{
if (i == qpoints.size() - 1)
painter.setBrush(Qt::blue); // Last = blue
painter.setBrush(Qt::blue); // Last = blue
else
painter.setBrush(Qt::red); // Middle = red
painter.setBrush(Qt::red); // Middle = red
painter.drawEllipse(qpoints[i], radius, radius);
}
@@ -224,7 +248,7 @@ namespace ModelecGUI
if (show_obstacle_)
{
for (auto & [index, obs] : obstacle_)
for (auto& [index, obs] : obstacle_)
{
painter.save();
@@ -233,8 +257,9 @@ namespace ModelecGUI
painter.rotate(90 - obs.theta * (180.0 / M_PI));
painter.setBrush(Qt::black);
QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
-(obs.height * ratioBetweenMapAndWidgetY_ / 2),
obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
painter.drawRect(toDraw);
@@ -245,7 +270,9 @@ namespace ModelecGUI
if (hasEnemy)
{
painter.setBrush(Qt::red);
painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, enemy_width_*ratioBetweenMapAndWidgetX_, enemy_length_*ratioBetweenMapAndWidgetY_);
painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_,
height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_,
enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_);
}
}
@@ -271,4 +298,4 @@ namespace ModelecGUI
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
}
}
}

141
src/modelec_gui/src/pages/odo_page.cpp
View File

@@ -4,25 +4,27 @@
namespace ModelecGUI
{
OdoPage::OdoPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), node_(node)
{
startButton_ = new QPushButton("Start");
connect(startButton_, &QPushButton::clicked, this, [this]() {
connect(startButton_, &QPushButton::clicked, this, [this]()
{
// Create a request for the speed service
RCLCPP_INFO(node_->get_logger(), "Start button clicked.");
auto request = std::make_shared<modelec_interfaces::srv::OdometryStart::Request>();
request->start = true;
client_start_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedFuture response) {
if (response.get()->success)
client_start_->async_send_request(
request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedFuture response)
{
RCLCPP_INFO(node_->get_logger(), "Start command sent successfully.");
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to send start command.");
}
});
if (response.get()->success)
{
RCLCPP_INFO(node_->get_logger(), "Start command sent successfully.");
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to send start command.");
}
});
});
// Initialize the UI components
@@ -40,25 +42,33 @@ namespace ModelecGUI
askPID_ = new QPushButton("Ask PID");
connect(askPID_, &QPushButton::clicked, this, [this]() {
connect(askPID_, &QPushButton::clicked, this, [this]()
{
RCLCPP_INFO(node_->get_logger(), "Ask PID button clicked.");
// Create a request for the PID service
auto request = std::make_shared<modelec_interfaces::srv::OdometryGetPid::Request>();
// Make the asynchronous service call
client_get_pid_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedFuture response) {
RCLCPP_INFO(node_->get_logger(), "Received PID response.");
if (auto res = response.get()) {
RCLCPP_INFO(node_->get_logger(), "PID values received: p=%f, i=%f, d=%f", res->p, res->i, res->d);
QMetaObject::invokeMethod(this, [this, res]() {
pPIDBox_->setValue(res->p);
iPIDBox_->setValue(res->i);
dPIDBox_->setValue(res->d);
});
} else {
RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request.");
}
});
client_get_pid_->async_send_request(
request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedFuture response)
{
RCLCPP_INFO(node_->get_logger(), "Received PID response.");
if (auto res = response.get())
{
RCLCPP_INFO(node_->get_logger(), "PID values received: p=%f, i=%f, d=%f", res->p, res->i,
res->d);
QMetaObject::invokeMethod(this, [this, res]()
{
pPIDBox_->setValue(res->p);
iPIDBox_->setValue(res->i);
dPIDBox_->setValue(res->d);
});
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request.");
}
});
});
pPIDBox_ = new QDoubleSpinBox(this);
pPIDBox_->setMinimum(0);
@@ -87,7 +97,8 @@ namespace ModelecGUI
pidLayout_->addWidget(dPIDBox_);
setPID_ = new QPushButton("Set PID");
connect(setPID_, &QPushButton::clicked, this, [this]() {
connect(setPID_, &QPushButton::clicked, this, [this]()
{
// Create a request for the PID service
auto request = std::make_shared<modelec_interfaces::srv::OdometrySetPid::Request>();
request->p = pPIDBox_->text().toFloat();
@@ -95,35 +106,44 @@ namespace ModelecGUI
request->d = dPIDBox_->text().toFloat();
// Make the asynchronous service call
client_set_pid_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::SharedFuture response) {
if (response.get()->success)
client_set_pid_->async_send_request(
request, [this](rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::SharedFuture response)
{
RCLCPP_INFO(node_->get_logger(), "Set PID command sent successfully.");
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to send set PID command.");
}
});
if (response.get()->success)
{
RCLCPP_INFO(node_->get_logger(), "Set PID command sent successfully.");
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to send set PID command.");
}
});
});
askSpeed_ = new QPushButton("Ask speed");
connect(askSpeed_, &QPushButton::clicked, this, [this]() {
connect(askSpeed_, &QPushButton::clicked, this, [this]()
{
// Create a request for the speed service
auto request = std::make_shared<modelec_interfaces::srv::OdometrySpeed::Request>();
// Make the asynchronous service call
client_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedFuture response) {
if (auto res = response.get()) {
QMetaObject::invokeMethod(this, [this, res]() {
xSpeedBox_->setText(QString("x: %1").arg(res->x));
ySpeedBox_->setText(QString("y: %1").arg(res->y));
thetaSpeedBox_->setText(QString("theta: %1").arg(res->theta));
});
} else {
RCLCPP_ERROR(node_->get_logger(), "Failed to get response for speed request.");
}
});
client_->async_send_request(
request, [this](rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedFuture response)
{
if (auto res = response.get())
{
QMetaObject::invokeMethod(this, [this, res]()
{
xSpeedBox_->setText(QString("x: %1").arg(res->x));
ySpeedBox_->setText(QString("y: %1").arg(res->y));
thetaSpeedBox_->setText(QString("theta: %1").arg(res->theta));
});
}
else
{
RCLCPP_ERROR(node_->get_logger(), "Failed to get response for speed request.");
}
});
});
xSpeedBox_ = new QLineEdit("x speed: ?");
@@ -154,8 +174,10 @@ namespace ModelecGUI
std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));
client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("odometry/speed");
while (!client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
@@ -163,8 +185,10 @@ namespace ModelecGUI
}
client_start_ = node_->create_client<modelec_interfaces::srv::OdometryStart>("odometry/start");
while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!client_start_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
@@ -173,8 +197,10 @@ namespace ModelecGUI
client_get_pid_ = node_->create_client<modelec_interfaces::srv::OdometryGetPid>("odometry/get_pid");
while (!client_get_pid_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!client_get_pid_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
@@ -182,8 +208,10 @@ namespace ModelecGUI
}
client_set_pid_ = node_->create_client<modelec_interfaces::srv::OdometrySetPid>("odometry/set_pid");
while (!client_set_pid_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!client_set_pid_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
@@ -195,7 +223,8 @@ namespace ModelecGUI
void OdoPage::PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
QMetaObject::invokeMethod(this, [this, msg]() {
QMetaObject::invokeMethod(this, [this, msg]()
{
xBox_->setText(QString("x: %1").arg(msg->x));
yBox_->setText(QString("y: %1").arg(msg->y));
thetaBox_->setText(QString("theta: %1").arg(msg->theta));

74
src/modelec_gui/src/pages/test_map_page.cpp
View File

@@ -8,7 +8,11 @@
namespace ModelecGUI
{
TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent), renderer(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)), node_(node)
TestMapPage::TestMapPage(rclcpp::Node::SharedPtr node, QWidget* parent) : QWidget(parent),
renderer(new QSvgRenderer(
QString(
":/img/playmat/2025_FINAL.svg"),
this)), node_(node)
{
ratioBetweenMapAndWidgetX_ = width() / 3000.0f;
ratioBetweenMapAndWidgetY_ = height() / 2000.0f;
@@ -21,14 +25,17 @@ namespace ModelecGUI
enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>("odometry/add_waypoint", 100,
[this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) {
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
"odometry/add_waypoint", 100,
[this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
{
if (lastWapointWasEnd)
{
qpoints.clear();
lastWapointWasEnd = false;
qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, height() - robotPos.y * ratioBetweenMapAndWidgetY_));
qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_,
height() - robotPos.y * ratioBetweenMapAndWidgetY_));
}
if (msg->is_end)
@@ -36,25 +43,30 @@ namespace ModelecGUI
lastWapointWasEnd = true;
}
qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, height() - msg->y * ratioBetweenMapAndWidgetY_));
qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_,
height() - msg->y * ratioBetweenMapAndWidgetY_));
update();
});
});
odometry_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>("odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
robotPos = *msg;
update();
});
});
obstacle_added_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/added", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
OnObstacleReceived(msg);
});
});
obstacle_removed_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>("nav/obstacle/removed", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) {
obstacle_removed_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
"nav/obstacle/removed", 40,
[this](const modelec_interfaces::msg::Obstacle::SharedPtr msg)
{
obstacle_.erase(msg->id);
});
});
go_to_pub_ = node_->create_publisher<modelec_interfaces::msg::OdometryGoTo>("nav/go_to", 10);
@@ -71,8 +83,10 @@ namespace ModelecGUI
// client to nav/map
map_client_ = node_->create_client<modelec_interfaces::srv::MapSize>("nav/map_size");
while (!map_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!map_client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
@@ -92,8 +106,10 @@ namespace ModelecGUI
}
ask_map_obstacle_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_map_obstacle");
while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
while (!ask_map_obstacle_client_->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(node_->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
@@ -138,7 +154,8 @@ namespace ModelecGUI
// --- Draw lines ---
painter.setPen(QPen(Qt::red, 2)); // Red lines, 2px wide
for (size_t i = 0; i + 1 < qpoints.size(); ++i) {
for (size_t i = 0; i + 1 < qpoints.size(); ++i)
{
painter.drawLine(qpoints[i], qpoints[i + 1]);
}
@@ -147,11 +164,12 @@ namespace ModelecGUI
// --- Draw colored points ---
const int radius = 5;
for (size_t i = 0; i < qpoints.size(); ++i) {
for (size_t i = 0; i < qpoints.size(); ++i)
{
if (i == qpoints.size() - 1)
painter.setBrush(Qt::blue); // Last = blue
painter.setBrush(Qt::blue); // Last = blue
else
painter.setBrush(Qt::red); // Middle = red
painter.setBrush(Qt::red); // Middle = red
painter.drawEllipse(qpoints[i], radius, radius);
}
@@ -160,7 +178,7 @@ namespace ModelecGUI
if (show_obstacle_)
{
for (auto & [index, obs] : obstacle_)
for (auto& [index, obs] : obstacle_)
{
painter.save();
@@ -169,8 +187,9 @@ namespace ModelecGUI
painter.rotate(90 - obs.theta * (180.0 / M_PI));
painter.setBrush(Qt::black);
QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2), -(obs.height * ratioBetweenMapAndWidgetY_ / 2),
obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
QRect toDraw(-(obs.width * ratioBetweenMapAndWidgetX_ / 2),
-(obs.height * ratioBetweenMapAndWidgetY_ / 2),
obs.width * ratioBetweenMapAndWidgetX_, obs.height * ratioBetweenMapAndWidgetY_);
painter.drawRect(toDraw);
@@ -181,7 +200,9 @@ namespace ModelecGUI
if (hasEnemy)
{
painter.setBrush(Qt::red);
painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_, height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_, enemy_width_*ratioBetweenMapAndWidgetX_, enemy_length_*ratioBetweenMapAndWidgetY_);
painter.drawRect((enemy_pos_.x - (enemy_width_ / 2)) * ratioBetweenMapAndWidgetX_,
height() - (enemy_pos_.y + (enemy_length_ / 2)) * ratioBetweenMapAndWidgetY_,
enemy_width_ * ratioBetweenMapAndWidgetX_, enemy_length_ * ratioBetweenMapAndWidgetY_);
}
}
@@ -212,7 +233,8 @@ namespace ModelecGUI
}
else
{
msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL | modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY;
msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL |
modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY;
}
go_to_pub_->publish(msg);

17
src/modelec_strat/data/config.xml
View File

@@ -32,25 +32,30 @@
</map>
<home>
<blue x="2650" y="1600" theta="-1.5708" />
<yellow x="350" y="1600" theta="-1.5708" />
<blue x="2650" y="1600" theta="-1.5708"/>
<yellow x="350" y="1600" theta="-1.5708"/>
</home>
<spawn>
<blue x="1775" y="300" theta="1.5708" />
<yellow x="1225" y="300" theta="1.5708" />
<blue x="1775" y="300" theta="1.5708"/>
<yellow x="1225" y="300" theta="1.5708"/>
</spawn>
<usb>
<pcb>
<pcb_odo>
<name>pcb_odo</name>
<port>/dev/pts/7</port>
<port>/dev/pts/9</port>
<baud_rate>115200</baud_rate>
</pcb_odo>
<pcb_alim>
<name>pcb_alim</name>
<port>/dev/pts/11</port>
<port>/dev/pts/12</port>
<baud_rate>115200</baud_rate>
</pcb_alim>
<pcb_action>
<name>pcb_action</name>
<port>/dev/pts/12</port>
<baud_rate>115200</baud_rate>
</pcb_action>
</pcb>
<timeout_ms>300</timeout_ms>
<attempt>5</attempt>

28
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

@@ -9,9 +9,10 @@
#include "deposite_zone.hpp"
#include "pathfinding.hpp"
namespace Modelec {
class NavigationHelper {
namespace Modelec
{
class NavigationHelper
{
public:
enum
{
@@ -30,15 +31,15 @@ namespace Modelec {
int GetTeamId() const;
void SendWaypoint() const;
void SendWaypoint(const std::vector<WaypointMsg> &waypoints) const;
void SendWaypoint(const std::vector<WaypointMsg>& waypoints) const;
void AddWaypoint(const PosMsg &pos, int index);
void AddWaypoint(const WaypointMsg &waypoint);
void AddWaypoint(const PosMsg& pos, int index);
void AddWaypoint(const WaypointMsg& waypoint);
void AddWaypoints(const std::initializer_list<PosMsg> &pos_list, int index);
void AddWaypoints(const std::initializer_list<WaypointMsg> &waypoint_list);
void AddWaypoints(const std::initializer_list<PosMsg>& pos_list, int index);
void AddWaypoints(const std::initializer_list<WaypointMsg>& waypoint_list);
void SetWaypoints(const std::list<Waypoint> &waypoints);
void SetWaypoints(const std::list<Waypoint>& waypoints);
bool HasArrived() const;
@@ -67,9 +68,10 @@ namespace Modelec {
PosMsg::SharedPtr GetCurrentPos() const;
bool LoadDepositeZoneFromXML(const std::string &filename);
bool LoadDepositeZoneFromXML(const std::string& filename);
std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr &pos, int teamId, const std::vector<int>& blacklistedId = {});
std::shared_ptr<DepositeZone> GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId,
const std::vector<int>& blacklistedId = {});
PosMsg::SharedPtr GetHomePosition();
@@ -78,7 +80,7 @@ namespace Modelec {
void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
bool DoesLineIntersectCircle(const Point& start, const Point& end,
const Point& center, float radius);
const Point& center, float radius);
bool EnemyOnPath(const modelec_interfaces::msg::OdometryPos msg);
@@ -136,4 +138,4 @@ namespace Modelec {
bool await_rotate_ = false;
std::vector<Waypoint> send_back_waypoints_;
};
}
}

21
src/modelec_strat/include/modelec_strat/pathfinding.hpp
View File

@@ -19,8 +19,8 @@
#include "obstacle/column.hpp"
namespace Modelec {
namespace Modelec
{
using WaypointMsg = modelec_interfaces::msg::OdometryAddWaypoint;
using WaypointReachMsg = modelec_interfaces::msg::OdometryWaypointReach;
using PosMsg = modelec_interfaces::msg::OdometryPos;
@@ -38,7 +38,8 @@ namespace Modelec {
WaypointMsg toMsg() const;
};
class Pathfinding {
class Pathfinding
{
public:
enum
{
@@ -83,10 +84,11 @@ namespace Modelec {
void AddObstacle(const std::shared_ptr<Obstacle>& obstacle);
template <typename T,
typename = std::enable_if_t<std::is_base_of<Obstacle, T>::value>>
typename = std::enable_if_t<std::is_base_of<Obstacle, T>::value>>
std::shared_ptr<T> GetClosestObstacle(const PosMsg::SharedPtr& pos) const;
std::shared_ptr<ColumnObstacle> GetClosestColumn(const PosMsg::SharedPtr& pos, const std::vector<int>& blacklistedId = {});
std::shared_ptr<ColumnObstacle> GetClosestColumn(const PosMsg::SharedPtr& pos,
const std::vector<int>& blacklistedId = {});
void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
@@ -145,10 +147,13 @@ namespace Modelec {
auto robotPos = Point(pos->x, pos->y, pos->theta);
float distance = std::numeric_limits<float>::max();
for (const auto& obstacle : obstacle_map_) {
if (auto obs = std::dynamic_pointer_cast<T>(obstacle.second)) {
for (const auto& obstacle : obstacle_map_)
{
if (auto obs = std::dynamic_pointer_cast<T>(obstacle.second))
{
auto dist = Point::distance(robotPos, obs->position());
if (dist < distance) {
if (dist < distance)
{
distance = dist;
closest_obstacle = obs;
}

34
src/modelec_strat/src/enemy_manager.cpp
View File

@@ -21,13 +21,15 @@ namespace Modelec
current_pos_sub_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
OnCurrentPos(msg);
});
laser_scan_sub_ = this->create_subscription<sensor_msgs::msg::LaserScan>(
"scan", 10,
[this](const sensor_msgs::msg::LaserScan::SharedPtr msg) {
[this](const sensor_msgs::msg::LaserScan::SharedPtr msg)
{
OnLidarData(msg);
});
@@ -35,13 +37,17 @@ namespace Modelec
"enemy/position", 10);
state_sub_ = create_subscription<modelec_interfaces::msg::StratState>("/strat/state", 10,
[this](const modelec_interfaces::msg::StratState::SharedPtr msg)
{
if (!game_stated_ && msg->state == modelec_interfaces::msg::StratState::SELECT_MISSION)
{
game_stated_ = true;
}
});
[this](
const
modelec_interfaces::msg::StratState::SharedPtr
msg)
{
if (!game_stated_ && msg->state ==
modelec_interfaces::msg::StratState::SELECT_MISSION)
{
game_stated_ = true;
}
});
enemy_long_time_pub_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"/enemy/long_time", 10);
@@ -142,7 +148,7 @@ namespace Modelec
{
last_enemy_pos_ = enemy_pos;
last_publish_time_ = this->now();
last_movement_time_ = this->now(); // Mise à jour du dernier vrai mouvement
last_movement_time_ = this->now(); // Mise à jour du dernier vrai mouvement
enemy_pos_pub_->publish(enemy_pos);
RCLCPP_INFO(this->get_logger(), "Enemy moved: x=%d, y=%d", enemy_pos.x, enemy_pos.y);
}
@@ -152,7 +158,8 @@ namespace Modelec
if ((now - last_movement_time_).seconds() > max_stationary_time_s_)
{
enemy_long_time_pub_->publish(last_enemy_pos_);
RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%d y=%d", last_enemy_pos_.x, last_enemy_pos_.y);
RCLCPP_WARN(this->get_logger(), "Enemy has been stationary for too long at x=%d y=%d",
last_enemy_pos_.x, last_enemy_pos_.y);
last_movement_time_ = now;
}
@@ -179,9 +186,10 @@ namespace Modelec
}
}
int main(int argc, char **argv) {
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::EnemyManager>());
rclcpp::shutdown();
return 0;
}
}

5
src/modelec_strat/src/missions/banner_mission.cpp
View File

@@ -4,7 +4,8 @@
namespace Modelec
{
BannerMission::BannerMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_FRONT), status_(MissionStatus::READY), nav_(nav)
BannerMission::BannerMission(const std::shared_ptr<NavigationHelper>& nav) : step_(GO_TO_FRONT),
status_(MissionStatus::READY), nav_(nav)
{
}
@@ -41,7 +42,7 @@ namespace Modelec
{
auto spawn = nav_->GetSpawn();
nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_/2) + 150, M_PI_2, true);
nav_->GoTo(spawn.x, (nav_->GetPathfinding()->robot_length_mm_ / 2) + 150, M_PI_2, true);
step_ = GO_FORWARD;
break;

74
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -4,7 +4,8 @@
#include "../../modelec_utils/include/modelec_utils/config.hpp"
namespace Modelec {
namespace Modelec
{
NavigationHelper::NavigationHelper()
{
}
@@ -25,7 +26,8 @@ namespace Modelec {
waypoint_reach_sub_ = node_->create_subscription<WaypointReachMsg>(
"odometry/waypoint_reach", 10,
[this](const WaypointReachMsg::SharedPtr msg) {
[this](const WaypointReachMsg::SharedPtr msg)
{
OnWaypointReach(msg);
});
@@ -33,14 +35,16 @@ namespace Modelec {
pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 20,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) {
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
OnPos(msg);
});
pos_pub_ = node_->create_publisher<PosMsg>("odometry/set_position", 10);
go_to_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryGoTo>(
"nav/go_to", 10, [this](const modelec_interfaces::msg::OdometryGoTo::SharedPtr msg) {
"nav/go_to", 10, [this](const modelec_interfaces::msg::OdometryGoTo::SharedPtr msg)
{
GoTo(msg->x, msg->y, msg->theta, msg->close, msg->mask);
});
@@ -58,7 +62,8 @@ namespace Modelec {
OnEnemyPositionLongTime(msg);
});
std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml";
std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
"/data/deposite_zone.xml";
if (!LoadDepositeZoneFromXML(deposite_zone_path))
{
RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML");
@@ -82,7 +87,7 @@ namespace Modelec {
void NavigationHelper::SendWaypoint() const
{
for (auto & w : waypoints_)
for (auto& w : waypoints_)
{
waypoint_pub_->publish(w.toMsg());
}
@@ -90,7 +95,7 @@ namespace Modelec {
void NavigationHelper::SendWaypoint(const std::vector<WaypointMsg>& waypoints) const
{
for (auto & w : waypoints)
for (auto& w : waypoints)
{
waypoint_pub_->publish(w);
}
@@ -178,7 +183,7 @@ namespace Modelec {
}
waypoints_.back().is_end = false;
for (auto & w : waypoint_list)
for (auto& w : waypoint_list)
{
waypoints_.emplace_back(w);
}
@@ -251,7 +256,7 @@ namespace Modelec {
waypoints_.clear();
for (auto & w : wl)
for (auto& w : wl)
{
waypoints_.emplace_back(w);
}
@@ -293,7 +298,7 @@ namespace Modelec {
send_back_waypoints_.clear();
for (auto & w : wl)
for (auto& w : wl)
{
send_back_waypoints_.emplace_back(w);
}
@@ -302,7 +307,7 @@ namespace Modelec {
{
waypoints_.clear();
for (auto & w : wl)
for (auto& w : wl)
{
waypoints_.emplace_back(w);
}
@@ -404,7 +409,8 @@ namespace Modelec {
return true;
}
std::shared_ptr<DepositeZone> NavigationHelper::GetClosestDepositeZone(const PosMsg::SharedPtr& pos, int teamId, const std::vector<int>& blacklistedId)
std::shared_ptr<DepositeZone> NavigationHelper::GetClosestDepositeZone(
const PosMsg::SharedPtr& pos, int teamId, const std::vector<int>& blacklistedId)
{
// TODO : score
std::shared_ptr<DepositeZone> closest_zone = nullptr;
@@ -414,7 +420,8 @@ namespace Modelec {
for (const auto& [id, zone] : deposite_zones_)
{
if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find(blacklistedId.begin(), blacklistedId.end(), id))
if (zone->GetTeam() == teamId && zone->RemainingPotPos() > 0 && blacklistedId.end() == std::find(
blacklistedId.begin(), blacklistedId.end(), id))
{
double distance = Point::distance(posPoint, zone->GetPosition());
double enemy_distance = Point::distance(enemyPos, zone->GetPosition());
@@ -468,9 +475,11 @@ namespace Modelec {
for (auto& [id, zone] : deposite_zones_)
{
auto zonePos = zone->GetPosition();
if (Point::distance(enemy_pos, zonePos) < pathfinding_->robot_width_mm_ + (zone->GetWidth()/2) + pathfinding_->enemy_margin_mm_)
if (Point::distance(enemy_pos, zonePos) < pathfinding_->robot_width_mm_ + (zone->GetWidth() / 2) +
pathfinding_->enemy_margin_mm_)
{
std::shared_ptr<Obstacle> obs = std::make_shared<Obstacle>(id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone");
std::shared_ptr<Obstacle> obs = std::make_shared<Obstacle>(
id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone");
pathfinding_->AddObstacle(obs);
}
}
@@ -496,10 +505,11 @@ namespace Modelec {
auto wp = waypointsList[i];
auto next_wp = waypointsList[i + 1];
if (DoesLineIntersectCircle(
Point(wp.x, wp.y, wp.theta),
Point(next_wp.x, next_wp.y, next_wp.theta),
Point(msg.x, msg.y, msg.theta),
(pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) / 2.0f))
Point(wp.x, wp.y, wp.theta),
Point(next_wp.x, next_wp.y, next_wp.theta),
Point(msg.x, msg.y, msg.theta),
(pathfinding_->enemy_length_mm_ + pathfinding_->robot_length_mm_ + pathfinding_->enemy_margin_mm_) /
2.0f))
{
return true;
}
@@ -508,9 +518,11 @@ namespace Modelec {
return false;
}
bool NavigationHelper::DoesLineIntersectCircle(const Point& start, const Point& end, const Point& center, float radius)
bool NavigationHelper::DoesLineIntersectCircle(const Point& start, const Point& end, const Point& center,
float radius)
{
float num = std::abs((end.y - start.y) * center.x - (end.x - start.x) * center.y + end.x * start.y - end.y * start.x);
float num = std::abs(
(end.y - start.y) * center.x - (end.x - start.x) * center.y + end.x * start.y - end.y * start.x);
float den = std::sqrt((end.y - start.y) * (end.y - start.y) + (end.x - start.x) * (end.x - start.x));
float dist = num / den;
return dist < radius;
@@ -524,7 +536,8 @@ namespace Modelec {
{
if (GoTo(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE)
{
GoTo(current_pos_, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY);
GoTo(current_pos_, true,
Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY);
// TODO : Handle case where no path is found
}
}
@@ -566,18 +579,18 @@ namespace Modelec {
{
switch (team_id_)
{
case YELLOW:
return spawn_yellow_;
case BLUE:
return spawn_blue_;
default:
return spawn_yellow_;
case YELLOW:
return spawn_yellow_;
case BLUE:
return spawn_blue_;
default:
return spawn_yellow_;
}
}
void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr msg)
{
for (auto & waypoint : waypoints_)
for (auto& waypoint : waypoints_)
{
if (waypoint.id == msg->id)
{
@@ -588,7 +601,7 @@ namespace Modelec {
await_rotate_ = false;
waypoints_.clear();
for (auto & w : send_back_waypoints_)
for (auto& w : send_back_waypoints_)
{
waypoints_.emplace_back(w);
}
@@ -603,5 +616,4 @@ namespace Modelec {
current_pos_ = msg;
pathfinding_->SetCurrentPos(msg);
}
}

9
src/modelec_strat/src/pami_manager.cpp
View File

@@ -56,7 +56,7 @@ namespace Modelec
second_goal - now,
[this]()
{
for (const auto & obs : zones_)
for (const auto& obs : zones_)
{
add_obs_pub_->publish(obs.toMsg());
@@ -108,8 +108,8 @@ namespace Modelec
}
for (tinyxml2::XMLElement* elem = root->FirstChildElement("zone");
elem;
elem = elem->NextSiblingElement("zone"))
elem;
elem = elem->NextSiblingElement("zone"))
{
zones_.push_back(Obstacle(elem));
}
@@ -119,7 +119,8 @@ namespace Modelec
}
}
int main(int argc, char **argv) {
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::PamiManger>());
rclcpp::shutdown();

3
src/modelec_strat/src/pathfinding.cpp
View File

@@ -606,7 +606,8 @@ namespace Modelec
{
if (auto column = std::dynamic_pointer_cast<ColumnObstacle>(obs))
{
if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->width() / 2) + enemy_margin_mm_)
if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->width() / 2) +
enemy_margin_mm_)
{
RemoveObstacle(column->id());
}

1
src/modelec_utils/src/point.cpp
View File

@@ -31,5 +31,4 @@ namespace Modelec
{
return GetTakePosition(210, theta);
}
}