diff --git a/src/modelec/include/modelec/button_gpio_controller.hpp b/src/modelec/include/modelec/button_gpio_controller.hpp deleted file mode 100644 index fc53958..0000000 --- a/src/modelec/include/modelec/button_gpio_controller.hpp +++ /dev/null @@ -1,35 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace Modelec { - class ButtonGpioController : public rclcpp::Node { - - struct Button { - int pin; - rclcpp::Publisher::SharedPtr publisher; - std::string name; - }; - - public: - ButtonGpioController(); - - private: - // service - rclcpp::Service::SharedPtr new_button_service_; - rclcpp::Service::SharedPtr button_server_; - - // service callbacks - void new_button(const std::shared_ptr request, std::shared_ptr response); - void check_button(const std::shared_ptr request, std::shared_ptr response); - - // timer - rclcpp::TimerBase::SharedPtr timer_; - - std::map buttons_; - }; -} \ No newline at end of file diff --git a/src/modelec/include/modelec/pcb_odo_interface.hpp b/src/modelec/include/modelec/pcb_odo_interface.hpp deleted file mode 100644 index 7a31d79..0000000 --- a/src/modelec/include/modelec/pcb_odo_interface.hpp +++ /dev/null @@ -1,157 +0,0 @@ -#pragma once - -#include - -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace Modelec { - -class PCBOdoInterface : public rclcpp::Node { -public: - PCBOdoInterface(); - - rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; - std::shared_ptr pcb_executor_; - std::thread pcb_executor_thread_; - ~PCBOdoInterface() override; - - struct OdometryData { - long x; - long y; - long theta; - }; - - struct PIDData - { - float p; - float i; - float d; - }; - -private: - rclcpp::Publisher::SharedPtr pcb_publisher_; - rclcpp::Subscription::SharedPtr pcb_subscriber_; - - void PCBCallback(const std_msgs::msg::String::SharedPtr msg); - - rclcpp::Publisher::SharedPtr odo_pos_publisher_; - rclcpp::Publisher::SharedPtr odo_speed_publisher_; - rclcpp::Publisher::SharedPtr odo_tof_publisher_; - rclcpp::Publisher::SharedPtr odo_waypoint_reach_publisher_; - rclcpp::Publisher::SharedPtr odo_pid_publisher_; - - rclcpp::Subscription::SharedPtr odo_add_waypoint_subscriber_; - rclcpp::Subscription::SharedPtr odo_set_pos_subscriber_; - rclcpp::Subscription::SharedPtr odo_set_pid_subscriber_; - - void AddWaypointCallback(const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const; - void SetPosCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg) const; - void SetPIDCallback(const modelec_interface::msg::OdometryPid::SharedPtr msg) const; - - rclcpp::Service::SharedPtr get_tof_service_; - rclcpp::Service::SharedPtr get_speed_service_; - rclcpp::Service::SharedPtr get_position_service_; - rclcpp::Service::SharedPtr set_start_service_; - rclcpp::Service::SharedPtr get_pid_service_; - rclcpp::Service::SharedPtr set_pid_service_; - rclcpp::Service::SharedPtr add_waypoint_service_; - - // Promises and mutexes to synchronize service responses asynchronously - std::queue> tof_promises_; - std::mutex tof_mutex_; - - std::queue> speed_promises_; - std::mutex speed_mutex_; - - std::queue> pos_promises_; - std::mutex pos_mutex_; - - std::queue> start_promises_; - std::mutex start_mutex_; - - std::queue> get_pid_promises_; - std::mutex get_pid_mutex_; - - std::queue> set_pid_promises_; - std::mutex set_pid_mutex_; - - std::queue> add_waypoint_promises_; - std::mutex add_waypoint_mutex_; - - // Service handlers using async wait with promises - void HandleGetTof(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetSpeed(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPosition(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetStart(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPID(const std::shared_ptr request, - std::shared_ptr response); - - void HandleSetPID(const std::shared_ptr request, - std::shared_ptr response); - - void HandleAddWaypoint(const std::shared_ptr request, - std::shared_ptr 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); - -public: - void SendToPCB(const std::string &data) const; - void SendToPCB(const std::string& order, const std::string& elem, const std::vector& data = {}) const; - - void GetData(const std::string& elem, const std::vector& data = {}) const; - void SendOrder(const std::string &elem, const std::vector &data = {}) const; - - void GetPos() const; - void GetSpeed() const; - void GetToF(const int &tof) const; - - void SetRobotPos(const modelec_interface::msg::OdometryPos::SharedPtr msg) const; - void SetRobotPos(long x, long y, long theta) const; - - void AddWaypoint(modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const; - void AddWaypoint(int index, bool IsStopPoint, long x, long y, long theta) const; - - void SetStart(const modelec_interface::msg::OdometryStart::SharedPtr msg) const; - void SetStart(bool start) const; - - void GetPID() const; - void SetPID(const modelec_interface::msg::OdometryPid::SharedPtr msg) const; - void SetPID(float p, float i, float d) const; -}; - -} // namespace Modelec diff --git a/src/modelec/include/modelec/solenoid_controller.hpp b/src/modelec/include/modelec/solenoid_controller.hpp deleted file mode 100644 index a099026..0000000 --- a/src/modelec/include/modelec/solenoid_controller.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#pragma once - -#include -#include -#include -#include -#include - -namespace Modelec { - class SolenoidController : public rclcpp::Node { - public: - SolenoidController(); - private: - std::unordered_set solenoid_pin_; - - rclcpp::Subscription::SharedPtr solenoid_subscriber_; - - rclcpp::Service::SharedPtr add_solenoid_service_; - - void activateSolenoid(const modelec_interface::msg::Solenoid::SharedPtr msg); - - void addSolenoid(const std::shared_ptr request, std::shared_ptr response); - }; -} diff --git a/src/modelec/include/modelec/utils.hpp b/src/modelec/include/modelec/utils.hpp deleted file mode 100644 index c21df84..0000000 --- a/src/modelec/include/modelec/utils.hpp +++ /dev/null @@ -1,47 +0,0 @@ -#pragma once - -#include -#include -#include - -namespace Modelec { - #define PI 3.14159265358979323846 - - inline std::vector split(const std::string &s, char delim) { - std::vector result; - std::string token; - std::istringstream tokenStream(s); - while (std::getline(tokenStream, token, delim)) { - result.push_back(token); - } - return result; - } - - inline std::string join(const std::vector &v, const std::string &delim) { - std::string result; - for (size_t i = 0; i < v.size(); ++i) { - if (i != 0) { - result += delim; - } - result += v[i]; - } - return result; - } - - inline bool startsWith(const std::string &s, const std::string &prefix) { - return s.rfind(prefix, 0) == 0; - } - - inline bool endsWith(const std::string &s, const std::string &suffix) { - return s.size() >= suffix.size() && s.rfind(suffix) == s.size() - suffix.size(); - } - - inline bool contains(const std::string &s, const std::string &substring) { - return s.find(substring) != std::string::npos; - } - - template ::value, int>::type = 0> - T mapValue(T v, T v_min, T v_max, T v_min_prime, T v_max_prime) { - return v_min_prime + (((v - v_min) * (v_max_prime - v_min_prime)) / (v_max - v_min)); - } -} diff --git a/src/modelec/include/modelec/multiple_serial_listener.hpp b/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp similarity index 81% rename from src/modelec/include/modelec/multiple_serial_listener.hpp rename to src/modelec_com/include/modelec_com/multiple_serial_listener.hpp index 8e473d6..d28439e 100644 --- a/src/modelec/include/modelec/multiple_serial_listener.hpp +++ b/src/modelec_com/include/modelec_com/multiple_serial_listener.hpp @@ -3,8 +3,7 @@ #include #include #include -#include -#include +#include #include #include #include @@ -60,10 +59,10 @@ private: boost::asio::io_service io; - rclcpp::Service::SharedPtr add_serial_listener_service_; + rclcpp::Service::SharedPtr add_serial_listener_service_; void add_serial_listener( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector ¶meters); diff --git a/src/modelec/include/modelec/pca9685_controller.hpp b/src/modelec_com/include/modelec_com/pca9685_controller.hpp similarity index 81% rename from src/modelec/include/modelec/pca9685_controller.hpp rename to src/modelec_com/include/modelec_com/pca9685_controller.hpp index 9a62cd1..1806519 100644 --- a/src/modelec/include/modelec/pca9685_controller.hpp +++ b/src/modelec_com/include/modelec_com/pca9685_controller.hpp @@ -5,8 +5,8 @@ #include #include -#include -#include +#include +#include #include #define PCA9685_FREQUENCY 25000000.0 @@ -24,14 +24,14 @@ namespace Modelec { private: int fd; // File descriptor for I2C communication - rclcpp::Subscription::SharedPtr servo_subscriber_; + rclcpp::Subscription::SharedPtr servo_subscriber_; rclcpp::Subscription::SharedPtr clear_subscriber_; int frequency = 50; // Default PWM frequency (Hz) std::unordered_set active_servos; // service to add a servo - rclcpp::Service::SharedPtr add_servo_service_; + rclcpp::Service::SharedPtr add_servo_service_; OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_; rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector ¶meters); diff --git a/src/modelec/include/modelec/pcb_alim_interface.hpp b/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp similarity index 55% rename from src/modelec/include/modelec/pcb_alim_interface.hpp rename to src/modelec_com/include/modelec_com/pcb_alim_interface.hpp index f5f7c9f..4617488 100644 --- a/src/modelec/include/modelec/pcb_alim_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_alim_interface.hpp @@ -2,12 +2,12 @@ #include #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace Modelec { @@ -39,15 +39,15 @@ private: void PCBCallback(const std_msgs::msg::String::SharedPtr msg); - rclcpp::Subscription::SharedPtr pcb_emg_subscriber_; + rclcpp::Subscription::SharedPtr pcb_emg_subscriber_; - void PCBEmgCallback(const modelec_interface::msg::AlimEmg::SharedPtr msg) const; + void PCBEmgCallback(const modelec_interfaces::msg::AlimEmg::SharedPtr msg) const; - rclcpp::Service::SharedPtr pcb_out_service_; - rclcpp::Service::SharedPtr pcb_in_service_; - rclcpp::Service::SharedPtr pcb_bau_service_; - rclcpp::Service::SharedPtr pcb_emg_service_; - rclcpp::Service::SharedPtr pcb_temp_service_; + rclcpp::Service::SharedPtr pcb_out_service_; + rclcpp::Service::SharedPtr pcb_in_service_; + rclcpp::Service::SharedPtr pcb_bau_service_; + rclcpp::Service::SharedPtr pcb_emg_service_; + rclcpp::Service::SharedPtr pcb_temp_service_; std::queue> pcb_out_promises_; std::mutex pcb_out_mutex_; @@ -65,28 +65,28 @@ private: std::mutex pcb_temp_mutex_; void HandleGetPCBOutData( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void HandleSetPCBOutData( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void HandleGetPCBInData( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void HandleGetPCBBauData( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void HandleSetPCBEmgData( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void HandleGetPCBTempData( - const std::shared_ptr request, - std::shared_ptr response); + const std::shared_ptr request, + std::shared_ptr response); void ResolveGetPCBOutRequest(const PCBData& value); void ResolveSetPCBOutRequest(const PCBData& value); diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp new file mode 100644 index 0000000..01967dd --- /dev/null +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -0,0 +1,158 @@ +#pragma once + +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace Modelec { + +class PCBOdoInterface : public rclcpp::Node { +public: + PCBOdoInterface(); + + rclcpp::CallbackGroup::SharedPtr pcb_callback_group_; + std::shared_ptr pcb_executor_; + std::thread pcb_executor_thread_; + ~PCBOdoInterface() override; + + struct OdometryData { + long x; + long y; + long theta; + }; + + struct PIDData + { + float p; + float i; + float d; + }; + +private: + rclcpp::Publisher::SharedPtr pcb_publisher_; + rclcpp::Subscription::SharedPtr pcb_subscriber_; + + void PCBCallback(const std_msgs::msg::String::SharedPtr msg); + + rclcpp::Publisher::SharedPtr odo_pos_publisher_; + rclcpp::Publisher::SharedPtr odo_speed_publisher_; + rclcpp::Publisher::SharedPtr odo_tof_publisher_; + rclcpp::Publisher::SharedPtr odo_waypoint_reach_publisher_; + rclcpp::Publisher::SharedPtr odo_pid_publisher_; + + rclcpp::Subscription::SharedPtr odo_add_waypoint_subscriber_; + rclcpp::Subscription::SharedPtr odo_set_pos_subscriber_; + rclcpp::Subscription::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::SharedPtr get_tof_service_; + rclcpp::Service::SharedPtr get_speed_service_; + rclcpp::Service::SharedPtr get_position_service_; + rclcpp::Service::SharedPtr set_start_service_; + rclcpp::Service::SharedPtr get_pid_service_; + rclcpp::Service::SharedPtr set_pid_service_; + rclcpp::Service::SharedPtr add_waypoint_service_; + + // Promises and mutexes to synchronize service responses asynchronously + std::queue> tof_promises_; + std::mutex tof_mutex_; + + std::queue> speed_promises_; + std::mutex speed_mutex_; + + std::queue> pos_promises_; + std::mutex pos_mutex_; + + std::queue> start_promises_; + std::mutex start_mutex_; + + std::queue> get_pid_promises_; + std::mutex get_pid_mutex_; + + std::queue> set_pid_promises_; + std::mutex set_pid_mutex_; + + std::queue> add_waypoint_promises_; + std::mutex add_waypoint_mutex_; + + // Service handlers using async wait with promises + void HandleGetTof(const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetSpeed(const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPosition(const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetStart(const std::shared_ptr request, + std::shared_ptr response); + + void HandleGetPID(const std::shared_ptr request, + std::shared_ptr response); + + void HandleSetPID(const std::shared_ptr request, + std::shared_ptr response); + + void HandleAddWaypoint(const std::shared_ptr request, + std::shared_ptr 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); + +public: + void SendToPCB(const std::string &data) const; + void SendToPCB(const std::string& order, const std::string& elem, const std::vector& data = {}) const; + + void GetData(const std::string& elem, const std::vector& data = {}) const; + void SendOrder(const std::string &elem, const std::vector &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, long theta) const; + + void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const; + void AddWaypoint(int index, bool IsStopPoint, long x, long y, long 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 diff --git a/src/modelec/src/multiple_serial_listener.cpp b/src/modelec_com/src/multiple_serial_listener.cpp similarity index 95% rename from src/modelec/src/multiple_serial_listener.cpp rename to src/modelec_com/src/multiple_serial_listener.cpp index e0a11d0..caa4964 100644 --- a/src/modelec/src/multiple_serial_listener.cpp +++ b/src/modelec_com/src/multiple_serial_listener.cpp @@ -1,4 +1,4 @@ -#include "modelec/multiple_serial_listener.hpp" +#include "modelec_com/multiple_serial_listener.hpp" #include #include #include @@ -94,7 +94,7 @@ namespace Modelec MultipleSerialListener::MultipleSerialListener() : Node("multiple_serial_listener"), io() { - add_serial_listener_service_ = create_service( + add_serial_listener_service_ = create_service( "add_serial_listener", std::bind(&MultipleSerialListener::add_serial_listener, this, std::placeholders::_1, std::placeholders::_2)); } @@ -108,8 +108,8 @@ namespace Modelec } void MultipleSerialListener::add_serial_listener( - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { if (serial_listeners.find(request->name) != serial_listeners.end()) { diff --git a/src/modelec/src/pca9685_controller.cpp b/src/modelec_com/src/pca9685_controller.cpp similarity index 91% rename from src/modelec/src/pca9685_controller.cpp rename to src/modelec_com/src/pca9685_controller.cpp index a44af2b..5d0d842 100644 --- a/src/modelec/src/pca9685_controller.cpp +++ b/src/modelec_com/src/pca9685_controller.cpp @@ -1,4 +1,4 @@ -#include "modelec/pca9685_controller.hpp" +#include "modelec_com/pca9685_controller.hpp" namespace Modelec { @@ -18,8 +18,8 @@ namespace Modelec { initializePCA9685(); // Subscribe to topics for servo and solenoid control - servo_subscriber_ = this->create_subscription( - "servo_control", 10, [this](const modelec_interface::msg::PCA9685Servo::SharedPtr msg) { + servo_subscriber_ = this->create_subscription( + "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; @@ -32,9 +32,9 @@ namespace Modelec { this->clearAllDevices(); }); - add_servo_service_ = this->create_service( - "add_servo", [this](const modelec_interface::srv::AddServoMotor::Request::SharedPtr request, - modelec_interface::srv::AddServoMotor::Response::SharedPtr response) { + add_servo_service_ = this->create_service( + "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()) { active_servos.insert(request->pin); response->success = true; diff --git a/src/modelec/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp similarity index 84% rename from src/modelec/src/pcb_alim_interface.cpp rename to src/modelec_com/src/pcb_alim_interface.cpp index c54bfca..1e16200 100644 --- a/src/modelec/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -1,17 +1,17 @@ -#include -#include -#include +#include +#include +#include namespace Modelec { PCBAlimInterface::PCBAlimInterface() : Node("pcb_alim_interface") { // Service to create a new serial listener - auto request = std::make_shared(); + auto request = std::make_shared(); request->name = "pcb_alim"; request->bauds = 115200; request->serial_port = "/dev/pts/12"; // TODO : check the real serial port - auto client = this->create_client("add_serial_listener"); + auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) @@ -69,12 +69,12 @@ namespace Modelec RCLCPP_ERROR(this->get_logger(), "Service call failed"); } - pcb_out_service_ = create_service( + pcb_out_service_ = create_service( "alim/out", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { - if (request->enable != -1 && request->type == modelec_interface::srv::AlimOut::Request::STATE) + if (request->enable != -1 && request->type == modelec_interfaces::srv::AlimOut::Request::STATE) { HandleSetPCBOutData(request, response); } @@ -84,41 +84,41 @@ namespace Modelec } }); - pcb_in_service_ = create_service( + pcb_in_service_ = create_service( "alim/in", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleGetPCBInData(request, response); }); - pcb_bau_service_ = create_service( + pcb_bau_service_ = create_service( "alim/bau", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleGetPCBBauData(request, response); }); - pcb_emg_service_ = create_service( + pcb_emg_service_ = create_service( "alim/emg", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleSetPCBEmgData(request, response); }); - pcb_temp_service_ = create_service( + pcb_temp_service_ = create_service( "alim/temp", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleGetPCBTempData(request, response); }); - pcb_emg_subscriber_ = this->create_subscription( + pcb_emg_subscriber_ = this->create_subscription( "alim/emg", 10, - [this](const modelec_interface::msg::AlimEmg::SharedPtr msg) + [this](const modelec_interfaces::msg::AlimEmg::SharedPtr msg) { PCBEmgCallback(msg); }); @@ -207,13 +207,13 @@ namespace Modelec } } - void PCBAlimInterface::PCBEmgCallback(const modelec_interface::msg::AlimEmg::SharedPtr msg) const + void PCBAlimInterface::PCBEmgCallback(const modelec_interfaces::msg::AlimEmg::SharedPtr msg) const { SendOrder("EMG", {"STATE", msg->activate == true ? "1" : "0"}); } - void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr request, - std::shared_ptr response) + void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -231,8 +231,8 @@ namespace Modelec response->result = result.value; } - void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr request, - std::shared_ptr response) + void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -250,8 +250,8 @@ namespace Modelec response->result = result.value; } - void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr request, - std::shared_ptr response) + void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -269,8 +269,8 @@ namespace Modelec response->result = result.value; } - void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr, - std::shared_ptr response) + void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -287,8 +287,8 @@ namespace Modelec response->activate = result.activate; } - void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr request, - std::shared_ptr response) + void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -304,8 +304,8 @@ namespace Modelec } void PCBAlimInterface::HandleGetPCBTempData( - const std::shared_ptr, - std::shared_ptr response) + const std::shared_ptr, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); diff --git a/src/modelec/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp similarity index 79% rename from src/modelec/src/pcb_odo_interface.cpp rename to src/modelec_com/src/pcb_odo_interface.cpp index 7248848..bcf69a6 100644 --- a/src/modelec/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -1,17 +1,17 @@ -#include -#include -#include +#include +#include +#include namespace Modelec { PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface") { // Service to create a new serial listener - auto request = std::make_shared(); + auto request = std::make_shared(); request->name = "pcb_odo"; request->bauds = 115200; request->serial_port = "/dev/pts/7"; // TODO : check the real serial port - auto client = this->create_client("add_serial_listener"); + auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) @@ -68,95 +68,95 @@ namespace Modelec RCLCPP_ERROR(this->get_logger(), "Service call failed"); } - odo_pos_publisher_ = this->create_publisher( + odo_pos_publisher_ = this->create_publisher( "odometry/get_position", 10); - odo_speed_publisher_ = this->create_publisher( + odo_speed_publisher_ = this->create_publisher( "odometry/speed", 10); - odo_tof_publisher_ = this->create_publisher( + odo_tof_publisher_ = this->create_publisher( "odometry/tof", 10); - odo_waypoint_reach_publisher_ = this->create_publisher( + odo_waypoint_reach_publisher_ = this->create_publisher( "odometry/waypoint_reach", 10); - odo_pid_publisher_ = this->create_publisher( + odo_pid_publisher_ = this->create_publisher( "odometry/get_pid", 10); - odo_add_waypoint_subscriber_ = this->create_subscription( + odo_add_waypoint_subscriber_ = this->create_subscription( "odometry/add_waypoint", 10, - [this](const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) + [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) { AddWaypointCallback(msg); }); - odo_set_pos_subscriber_ = this->create_subscription( + odo_set_pos_subscriber_ = this->create_subscription( "odometry/set_position", 10, - [this](const modelec_interface::msg::OdometryPos::SharedPtr msg) + [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { SetPosCallback(msg); }); - odo_set_pid_subscriber_ = this->create_subscription( + odo_set_pid_subscriber_ = this->create_subscription( "odometry/set_pid", 10, - [this](const modelec_interface::msg::OdometryPid::SharedPtr msg) + [this](const modelec_interfaces::msg::OdometryPid::SharedPtr msg) { SetPIDCallback(msg); }); // Services - get_tof_service_ = create_service( + get_tof_service_ = create_service( "odometry/tof", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleGetTof(request, response); }); - get_speed_service_ = create_service( + get_speed_service_ = create_service( "odometry/speed", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleGetSpeed(request, response); }); - get_position_service_ = create_service( + get_position_service_ = create_service( "odometry/get_position", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleGetPosition(request, response); }); - set_start_service_ = create_service( + set_start_service_ = create_service( "odometry/start", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleGetStart(request, response); }); - get_pid_service_ = create_service( + get_pid_service_ = create_service( "odometry/get_pid", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleGetPID(request, response); }); - set_pid_service_ = create_service( + set_pid_service_ = create_service( "odometry/set_pid", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleSetPID(request, response); }); - add_waypoint_service_ = create_service( + add_waypoint_service_ = create_service( "odometry/add_waypoint", - [this](const std::shared_ptr request, - std::shared_ptr response) + [this](const std::shared_ptr request, + std::shared_ptr response) { HandleAddWaypoint(request, response); }); @@ -188,7 +188,7 @@ namespace Modelec long y = std::stol(tokens[3]); long theta = std::stol(tokens[4]); - auto message = modelec_interface::msg::OdometryPos(); + auto message = modelec_interfaces::msg::OdometryPos(); message.x = x; message.y = y; message.theta = theta; @@ -203,7 +203,7 @@ namespace Modelec long y = std::stol(tokens[3]); long theta = std::stol(tokens[4]); - auto message = modelec_interface::msg::OdometrySpeed(); + auto message = modelec_interfaces::msg::OdometrySpeed(); message.x = x; message.y = y; message.theta = theta; @@ -216,7 +216,7 @@ namespace Modelec int n = std::stoi(tokens[2]); long dist = std::stol(tokens[3]); - auto message = modelec_interface::msg::OdometryToF(); + auto message = modelec_interfaces::msg::OdometryToF(); message.n = n; message.distance = dist; @@ -227,7 +227,7 @@ namespace Modelec { int id = std::stoi(tokens[2]); - auto message = modelec_interface::msg::OdometryWaypointReach(); + auto message = modelec_interfaces::msg::OdometryWaypointReach(); message.id = id; odo_waypoint_reach_publisher_->publish(message); @@ -238,7 +238,7 @@ namespace Modelec float i = std::stof(tokens[3]); float d = std::stof(tokens[4]); - auto message = modelec_interface::msg::OdometryPid(); + auto message = modelec_interfaces::msg::OdometryPid(); message.p = p; message.i = i; message.d = d; @@ -295,24 +295,24 @@ namespace Modelec } } - void PCBOdoInterface::AddWaypointCallback(const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const + void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const { AddWaypoint(msg); } - void PCBOdoInterface::SetPosCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg) const + void PCBOdoInterface::SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const { SetRobotPos(msg); } - void PCBOdoInterface::SetPIDCallback(const modelec_interface::msg::OdometryPid::SharedPtr msg) const + void PCBOdoInterface::SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const { SetPID(msg); } void PCBOdoInterface::HandleGetTof( - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -328,8 +328,8 @@ namespace Modelec } void PCBOdoInterface::HandleGetSpeed( - const std::shared_ptr, - std::shared_ptr response) + const std::shared_ptr, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -348,8 +348,8 @@ namespace Modelec } void PCBOdoInterface::HandleGetPosition( - const std::shared_ptr, - std::shared_ptr response) + const std::shared_ptr, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -367,8 +367,8 @@ namespace Modelec response->theta = result.theta; } - void PCBOdoInterface::HandleGetStart(const std::shared_ptr request, - std::shared_ptr response) + void PCBOdoInterface::HandleGetStart(const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -382,8 +382,8 @@ namespace Modelec response->success = future.get(); } - void PCBOdoInterface::HandleGetPID(const std::shared_ptr, - std::shared_ptr response) + void PCBOdoInterface::HandleGetPID(const std::shared_ptr, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -402,8 +402,8 @@ namespace Modelec response->d = result.d; } - void PCBOdoInterface::HandleSetPID(const std::shared_ptr request, - std::shared_ptr response) + void PCBOdoInterface::HandleSetPID(const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -420,8 +420,8 @@ namespace Modelec } void PCBOdoInterface::HandleAddWaypoint( - const std::shared_ptr request, - std::shared_ptr response) + const std::shared_ptr request, + std::shared_ptr response) { std::promise promise; auto future = promise.get_future(); @@ -563,7 +563,7 @@ namespace Modelec GetData("DIST", {std::to_string(tof)}); } - void PCBOdoInterface::SetRobotPos(const modelec_interface::msg::OdometryPos::SharedPtr msg) const + void PCBOdoInterface::SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const { SetRobotPos(msg->x, msg->y, msg->theta); } @@ -580,7 +580,7 @@ namespace Modelec } void PCBOdoInterface::AddWaypoint( - const modelec_interface::msg::OdometryAddWaypoint::SharedPtr msg) const + const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const { AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta); } @@ -599,7 +599,7 @@ namespace Modelec SendOrder("WAYPOINT", data); } - void PCBOdoInterface::SetStart(const modelec_interface::msg::OdometryStart::SharedPtr msg) const + void PCBOdoInterface::SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const { SetStart(msg->start); } @@ -614,7 +614,7 @@ namespace Modelec GetData("PID"); } - void PCBOdoInterface::SetPID(const modelec_interface::msg::OdometryPid::SharedPtr msg) const + void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const { SetPID(msg->p, msg->i, msg->d); } diff --git a/src/modelec/CMakeLists.txt b/src/modelec_core/CMakeLists.txt similarity index 50% rename from src/modelec/CMakeLists.txt rename to src/modelec_core/CMakeLists.txt index 4360ce2..ea4c91a 100644 --- a/src/modelec/CMakeLists.txt +++ b/src/modelec_core/CMakeLists.txt @@ -1,115 +1,66 @@ cmake_minimum_required(VERSION 3.8) -project(modelec) +project(modelec_core) # Enable all warnings for GCC/Clang if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -# Set C++ standard -set(CMAKE_CXX_STANDARD 17) -set(CMAKE_CXX_STANDARD_REQUIRED ON) -set(CMAKE_CXX_EXTENSIONS OFF) - # Find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(Boost REQUIRED COMPONENTS system) -find_package(modelec_interface REQUIRED) +find_package(modelec_interfaces REQUIRED) +find_package(modelec_utils REQUIRED) find_library(WIRINGPI_LIB wiringPi) -# USB Arduino Listener Node -add_executable(serial_listener src/multiple_serial_listener.cpp) -ament_target_dependencies(serial_listener rclcpp std_msgs modelec_interface) -target_link_libraries(serial_listener Boost::system) -target_include_directories(serial_listener PUBLIC - $ - $ -) - -# USB Arduino Logic Processor Node -add_executable(pcb_odo_interface src/pcb_odo_interface.cpp) -ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interface) -target_include_directories(pcb_odo_interface PUBLIC - $ - $ -) - -# USB Arduino Logic Processor Node -add_executable(pcb_alim_interface src/pcb_alim_interface.cpp) -ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interface) -target_include_directories(pcb_alim_interface PUBLIC - $ - $ -) - -# PCA9685 Listener Node -add_executable(pca9685_controller src/pca9685_controller.cpp) -ament_target_dependencies(pca9685_controller rclcpp std_msgs modelec_interface) -target_link_libraries(pca9685_controller ${WIRINGPI_LIB}) -target_include_directories(pca9685_controller PUBLIC - $ - $ -) - add_executable(gamecontroller_listener src/gamecontroller_listener.cpp) -ament_target_dependencies(gamecontroller_listener rclcpp std_msgs sensor_msgs modelec_interface) +ament_target_dependencies(gamecontroller_listener rclcpp std_msgs sensor_msgs modelec_interfaces) +target_link_libraries(gamecontroller_listener modelec_utils::modelec_utils) target_include_directories(gamecontroller_listener PUBLIC $ $ ) add_executable(move_controller src/move_controller.cpp) -ament_target_dependencies(move_controller rclcpp std_msgs sensor_msgs modelec_interface) +ament_target_dependencies(move_controller rclcpp std_msgs sensor_msgs modelec_interfaces) +target_link_libraries(move_controller modelec_utils::modelec_utils) target_include_directories(move_controller PUBLIC $ $ ) -add_executable(tirette_controller src/tirette_controller.cpp) -ament_target_dependencies(tirette_controller rclcpp std_msgs modelec_interface) -target_link_libraries(tirette_controller ${WIRINGPI_LIB}) -target_include_directories(tirette_controller PUBLIC - $ - $ -) - add_executable(solenoid_controller src/solenoid_controller.cpp) -ament_target_dependencies(solenoid_controller rclcpp modelec_interface) -target_link_libraries(solenoid_controller ${WIRINGPI_LIB}) +ament_target_dependencies(solenoid_controller rclcpp modelec_interfaces) +target_link_libraries(solenoid_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils) target_include_directories(solenoid_controller PUBLIC $ $ ) add_executable(arm_controller src/arm_controller.cpp) -ament_target_dependencies(arm_controller rclcpp std_msgs modelec_interface) -target_link_libraries(arm_controller ${WIRINGPI_LIB}) +ament_target_dependencies(arm_controller rclcpp std_msgs modelec_interfaces) +target_link_libraries(arm_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils) target_include_directories(arm_controller PUBLIC $ $ ) add_executable(button_gpio_controller src/button_gpio_controller.cpp) -ament_target_dependencies(button_gpio_controller rclcpp std_msgs modelec_interface) -target_link_libraries(button_gpio_controller ${WIRINGPI_LIB}) +ament_target_dependencies(button_gpio_controller rclcpp std_msgs modelec_interfaces) +target_link_libraries(button_gpio_controller ${WIRINGPI_LIB} modelec_utils::modelec_utils) target_include_directories(button_gpio_controller PUBLIC $ $ ) -add_executable(lidar_controller src/lidar_controller.cpp) -ament_target_dependencies(lidar_controller rclcpp std_msgs sensor_msgs modelec_interface) -target_include_directories(lidar_controller PUBLIC - $ - $ -) - add_executable(speed_result src/speed_result.cpp) -ament_target_dependencies(speed_result rclcpp std_msgs sensor_msgs modelec_interface) +ament_target_dependencies(speed_result rclcpp std_msgs sensor_msgs modelec_interfaces) +target_link_libraries(speed_result modelec_utils::modelec_utils) target_include_directories(speed_result PUBLIC $ $ @@ -125,13 +76,8 @@ endif() # Install targets install(TARGETS - serial_listener - pcb_alim_interface - pcb_odo_interface - pca9685_controller gamecontroller_listener move_controller - tirette_controller solenoid_controller arm_controller button_gpio_controller diff --git a/src/modelec/LICENSE b/src/modelec_core/LICENSE similarity index 100% rename from src/modelec/LICENSE rename to src/modelec_core/LICENSE diff --git a/src/modelec/config/gz_bridge.yaml b/src/modelec_core/config/gz_bridge.yaml similarity index 100% rename from src/modelec/config/gz_bridge.yaml rename to src/modelec_core/config/gz_bridge.yaml diff --git a/src/modelec/config/rviz_view.rviz b/src/modelec_core/config/rviz_view.rviz similarity index 100% rename from src/modelec/config/rviz_view.rviz rename to src/modelec_core/config/rviz_view.rviz diff --git a/src/modelec/description/gazebo_control.xacro b/src/modelec_core/description/gazebo_control.xacro similarity index 100% rename from src/modelec/description/gazebo_control.xacro rename to src/modelec_core/description/gazebo_control.xacro diff --git a/src/modelec/description/inertial_macros.xacro b/src/modelec_core/description/inertial_macros.xacro similarity index 100% rename from src/modelec/description/inertial_macros.xacro rename to src/modelec_core/description/inertial_macros.xacro diff --git a/src/modelec/description/robot.urdf.xacro b/src/modelec_core/description/robot.urdf.xacro similarity index 100% rename from src/modelec/description/robot.urdf.xacro rename to src/modelec_core/description/robot.urdf.xacro diff --git a/src/modelec/description/robot_core.xacro b/src/modelec_core/description/robot_core.xacro similarity index 100% rename from src/modelec/description/robot_core.xacro rename to src/modelec_core/description/robot_core.xacro diff --git a/src/modelec/include/modelec/arm_controller.hpp b/src/modelec_core/include/modelec/arm_controller.hpp similarity index 73% rename from src/modelec/include/modelec/arm_controller.hpp rename to src/modelec_core/include/modelec/arm_controller.hpp index e57551f..7938713 100644 --- a/src/modelec/include/modelec/arm_controller.hpp +++ b/src/modelec_core/include/modelec/arm_controller.hpp @@ -2,9 +2,9 @@ #include #include -#include -#include -#include +#include +#include +#include #include #include @@ -17,7 +17,7 @@ namespace Modelec { class ArmController : public rclcpp::Node { - using Mode = modelec_interface::msg::ServoMode; + using Mode = modelec_interfaces::msg::ServoMode; struct Pince { int pin; @@ -34,8 +34,8 @@ namespace Modelec { public: ArmController(); private: - rclcpp::Publisher::SharedPtr servo_spublisher_; - rclcpp::Subscription::SharedPtr subscription_; + rclcpp::Publisher::SharedPtr servo_spublisher_; + rclcpp::Subscription::SharedPtr subscription_; std::unordered_map pince_pins = { {PINCE_1_PIN, {PINCE_1_PIN, Mode::PINCE_CLOSED, {136, 123, 112, 102}}}, @@ -52,7 +52,7 @@ namespace Modelec { }; // service lient to add a servo - rclcpp::Client::SharedPtr client_; + rclcpp::Client::SharedPtr client_; void ControlPince(const Mode::SharedPtr msg); void ControlArm(const Mode::SharedPtr msg); diff --git a/src/modelec_core/include/modelec/button_gpio_controller.hpp b/src/modelec_core/include/modelec/button_gpio_controller.hpp new file mode 100644 index 0000000..0abc738 --- /dev/null +++ b/src/modelec_core/include/modelec/button_gpio_controller.hpp @@ -0,0 +1,35 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace Modelec { + class ButtonGpioController : public rclcpp::Node { + + struct Button { + int pin; + rclcpp::Publisher::SharedPtr publisher; + std::string name; + }; + + public: + ButtonGpioController(); + + private: + // service + rclcpp::Service::SharedPtr new_button_service_; + rclcpp::Service::SharedPtr button_server_; + + // service callbacks + void new_button(const std::shared_ptr request, std::shared_ptr response); + void check_button(const std::shared_ptr request, std::shared_ptr response); + + // timer + rclcpp::TimerBase::SharedPtr timer_; + + std::map buttons_; + }; +} \ No newline at end of file diff --git a/src/modelec/include/modelec/gamecontroller_listener.hpp b/src/modelec_core/include/modelec/gamecontroller_listener.hpp similarity index 78% rename from src/modelec/include/modelec/gamecontroller_listener.hpp rename to src/modelec_core/include/modelec/gamecontroller_listener.hpp index 6eb397d..d6b513a 100644 --- a/src/modelec/include/modelec/gamecontroller_listener.hpp +++ b/src/modelec_core/include/modelec/gamecontroller_listener.hpp @@ -1,21 +1,20 @@ #pragma once -#include "modelec_interface/msg/pca9685_servo.hpp" +#include "modelec_interfaces/msg/pca9685_servo.hpp" #include #include #include #include #include #include -#include -#include -#include -#include +#include +#include +#include namespace Modelec { class ControllerListener : public rclcpp::Node { - using ServoMode = modelec_interface::msg::ServoMode; + using ServoMode = modelec_interfaces::msg::ServoMode; std::array pinces = { ServoMode::PINCE_CLOSED, @@ -45,8 +44,8 @@ namespace Modelec { rclcpp::Publisher::SharedPtr servo_publisher_; rclcpp::Publisher::SharedPtr odometry_publisher_; rclcpp::Publisher::SharedPtr clear_pca_publisher_; - rclcpp::Publisher::SharedPtr pca9685_publisher_; - rclcpp::Client::SharedPtr client_; + rclcpp::Publisher::SharedPtr pca9685_publisher_; + rclcpp::Client::SharedPtr client_; void CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg); void CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg); diff --git a/src/modelec/include/modelec/move_controller.hpp b/src/modelec_core/include/modelec/move_controller.hpp similarity index 100% rename from src/modelec/include/modelec/move_controller.hpp rename to src/modelec_core/include/modelec/move_controller.hpp diff --git a/src/modelec_core/include/modelec/solenoid_controller.hpp b/src/modelec_core/include/modelec/solenoid_controller.hpp new file mode 100644 index 0000000..31ecdd1 --- /dev/null +++ b/src/modelec_core/include/modelec/solenoid_controller.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include +#include +#include + +namespace Modelec { + class SolenoidController : public rclcpp::Node { + public: + SolenoidController(); + private: + std::unordered_set solenoid_pin_; + + rclcpp::Subscription::SharedPtr solenoid_subscriber_; + + rclcpp::Service::SharedPtr add_solenoid_service_; + + void activateSolenoid(const modelec_interfaces::msg::Solenoid::SharedPtr msg); + + void addSolenoid(const std::shared_ptr request, std::shared_ptr response); + }; +} diff --git a/src/modelec/include/modelec/speed_result.hpp b/src/modelec_core/include/modelec/speed_result.hpp similarity index 58% rename from src/modelec/include/modelec/speed_result.hpp rename to src/modelec_core/include/modelec/speed_result.hpp index 56bc032..7b152b7 100644 --- a/src/modelec/include/modelec/speed_result.hpp +++ b/src/modelec_core/include/modelec/speed_result.hpp @@ -1,7 +1,7 @@ #pragma once #include -#include +#include #include namespace Modelec @@ -19,8 +19,8 @@ namespace Modelec std::ofstream file_; - rclcpp::Subscription::SharedPtr sub_speed_; + rclcpp::Subscription::SharedPtr sub_speed_; - void SpeedCallback(const modelec_interface::msg::OdometrySpeed::SharedPtr msg); + void SpeedCallback(const modelec_interfaces::msg::OdometrySpeed::SharedPtr msg); }; } \ No newline at end of file diff --git a/src/modelec/launch/launch_sim.launch.py b/src/modelec_core/launch/launch_sim.launch.py similarity index 100% rename from src/modelec/launch/launch_sim.launch.py rename to src/modelec_core/launch/launch_sim.launch.py diff --git a/src/modelec/launch/modelec.launch.yml b/src/modelec_core/launch/modelec.launch.yml similarity index 87% rename from src/modelec/launch/modelec.launch.yml rename to src/modelec_core/launch/modelec.launch.yml index 93d8a74..88b37a8 100644 --- a/src/modelec/launch/modelec.launch.yml +++ b/src/modelec_core/launch/modelec.launch.yml @@ -1,22 +1,22 @@ launch: - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: "serial_listener" name: "serial_listener" - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pcb_odo_interface' name: 'pcb_odo_interface' - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pcb_alim_interface' name: 'pcb_alim_interface' - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pca9685_controller' name: 'pca9685_controller' diff --git a/src/modelec/launch/modelec_gamecontroller.launch.yml b/src/modelec_core/launch/modelec_gamecontroller.launch.yml similarity index 86% rename from src/modelec/launch/modelec_gamecontroller.launch.yml rename to src/modelec_core/launch/modelec_gamecontroller.launch.yml index e3699a8..778cb12 100644 --- a/src/modelec/launch/modelec_gamecontroller.launch.yml +++ b/src/modelec_core/launch/modelec_gamecontroller.launch.yml @@ -1,22 +1,22 @@ launch: - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: "serial_listener" name: "serial_listener" - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pcb_odo_interface' name: 'pcb_odo_interface' - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pcb_alim_interface' name: 'pcb_alim_interface' - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pca9685_controller' name: 'pca9685_controller' diff --git a/src/modelec/launch/modelec_gamecontroller_remote_controller.launch.yml b/src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml similarity index 100% rename from src/modelec/launch/modelec_gamecontroller_remote_controller.launch.yml rename to src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml diff --git a/src/modelec/launch/modelec_gamecontroller_remote_server.launch.yml b/src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml similarity index 83% rename from src/modelec/launch/modelec_gamecontroller_remote_server.launch.yml rename to src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml index 6ae75be..eb508d8 100644 --- a/src/modelec/launch/modelec_gamecontroller_remote_server.launch.yml +++ b/src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml @@ -1,22 +1,22 @@ launch: - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: "serial_listener" name: "serial_listener" - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pcb_odo_interface' name: 'pcb_odo_interface' - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pcb_alim_interface' name: 'pcb_alim_interface' - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pca9685_controller' name: 'pca9685_controller' diff --git a/src/modelec/launch/rsp.launch.py b/src/modelec_core/launch/rsp.launch.py similarity index 100% rename from src/modelec/launch/rsp.launch.py rename to src/modelec_core/launch/rsp.launch.py diff --git a/src/modelec/launch/test.modelec.launch.yml b/src/modelec_core/launch/test.modelec.launch.yml similarity index 84% rename from src/modelec/launch/test.modelec.launch.yml rename to src/modelec_core/launch/test.modelec.launch.yml index 139ac4a..af31724 100644 --- a/src/modelec/launch/test.modelec.launch.yml +++ b/src/modelec_core/launch/test.modelec.launch.yml @@ -1,17 +1,17 @@ launch: - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: "serial_listener" name: "serial_listener" - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pcb_odo_interface' name: 'pcb_odo_interface' - node: - pkg: 'modelec' + pkg: 'modelec_com' exec: 'pcb_alim_interface' name: 'pcb_alim_interface' diff --git a/src/modelec/package.xml b/src/modelec_core/package.xml similarity index 82% rename from src/modelec/package.xml rename to src/modelec_core/package.xml index 6b47f26..f3ad056 100644 --- a/src/modelec/package.xml +++ b/src/modelec_core/package.xml @@ -1,18 +1,20 @@ - modelec - 0.0.0 + modelec_core + 0.1.0 TODO: Package description modelec GPL-3.0-only ament_cmake - modelec_interface + modelec_interfaces + modelec_utils rclcpp std_msgs sensor_msgs + geometry_msgs boost boost diff --git a/src/modelec/src/arm_controller.cpp b/src/modelec_core/src/arm_controller.cpp similarity index 84% rename from src/modelec/src/arm_controller.cpp rename to src/modelec_core/src/arm_controller.cpp index 4a1126a..ff7eec2 100644 --- a/src/modelec/src/arm_controller.cpp +++ b/src/modelec_core/src/arm_controller.cpp @@ -1,10 +1,10 @@ -#include "modelec_interface/msg/servo_mode.hpp" +#include "modelec_interfaces/msg/servo_mode.hpp" #include namespace Modelec { ArmController::ArmController() : Node("pince_controller") { - this->servo_spublisher_ = this->create_publisher("servo_control", 10); - client_ = this->create_client("add_servo"); + this->servo_spublisher_ = this->create_publisher("servo_control", 10); + client_ = this->create_client("add_servo"); // ensure the server is up while (!client_->wait_for_service(std::chrono::seconds(1))) { @@ -16,7 +16,7 @@ namespace Modelec { } for (int pin : {PINCE_1_PIN, PINCE_2_PIN, PINCE_3_PIN, ARM_1_PIN, ARM_2_PIN}) { - auto request = std::make_shared(); + auto request = std::make_shared(); request->pin = pin; auto res = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) == rclcpp::FutureReturnCode::SUCCESS) { @@ -30,8 +30,8 @@ namespace Modelec { } } - subscription_ = this->create_subscription( - "arm_control", 10, [this](const modelec_interface::msg::ServoMode::SharedPtr msg) { + subscription_ = this->create_subscription( + "arm_control", 10, [this](const modelec_interfaces::msg::ServoMode::SharedPtr msg) { if (msg->is_arm) { ControlArm(msg); } else if (pince_pins.find(msg->pin) != pince_pins.end()) { @@ -47,7 +47,7 @@ namespace Modelec { return; } - auto message = modelec_interface::msg::PCA9685Servo(); + auto message = modelec_interfaces::msg::PCA9685Servo(); message.pin = msg->pin; message.angle = pince.angles[msg->mode]; servo_spublisher_->publish(message); @@ -62,7 +62,7 @@ namespace Modelec { for (auto pince : pince_pins) { if (pince.second.mode != Mode::PINCE_CLOSED) { - auto message = modelec_interface::msg::PCA9685Servo(); + auto message = modelec_interfaces::msg::PCA9685Servo(); message.pin = pince.second.pin; message.angle = pince.second.angles[msg->mode]; servo_spublisher_->publish(message); @@ -83,7 +83,7 @@ namespace Modelec { int startAngle = arm.pins[ARM_1_PIN][arm.mode]; for (int angle = startAngle; angle <= arm.pins[ARM_1_PIN][msg->mode]; angle += 2 * direction) { - auto message = modelec_interface::msg::PCA9685Servo(); + auto message = modelec_interfaces::msg::PCA9685Servo(); message.pin = ARM_1_PIN; message.angle = angle; publisher_->publish(message); @@ -94,7 +94,7 @@ namespace Modelec { std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - auto message = modelec_interface::msg::PCA9685Servo(); + auto message = modelec_interfaces::msg::PCA9685Servo(); message.pin = ARM_1_PIN; message.angle = arm.pins[ARM_1_PIN][msg->mode]; publisher_->publish(message); diff --git a/src/modelec/src/button_gpio_controller.cpp b/src/modelec_core/src/button_gpio_controller.cpp similarity index 62% rename from src/modelec/src/button_gpio_controller.cpp rename to src/modelec_core/src/button_gpio_controller.cpp index b02272c..178e158 100644 --- a/src/modelec/src/button_gpio_controller.cpp +++ b/src/modelec_core/src/button_gpio_controller.cpp @@ -1,57 +1,57 @@ -#include - -namespace Modelec { - ButtonGpioController::ButtonGpioController() : Node("button_gpio_controller") { - if (wiringPiSetupGpio() == -1) - { - RCLCPP_ERROR(this->get_logger(), "Failed to initialize GPIO."); - rclcpp::shutdown(); - } - - new_button_service_ = create_service("add_button", std::bind(&ButtonGpioController::new_button, this, std::placeholders::_1, std::placeholders::_2)); - button_server_ = create_service("button", std::bind(&ButtonGpioController::check_button, this, std::placeholders::_1, std::placeholders::_2)); - timer_ = create_wall_timer(std::chrono::seconds(1), [this]() { - for (auto& button : buttons_) { - modelec_interface::msg::Button msg; - msg.pin = button.second.pin; - msg.state = digitalRead(button.second.pin) == LOW; - button.second.publisher->publish(msg); - } - }); - } - - void ButtonGpioController::new_button(const std::shared_ptr request, std::shared_ptr response) { - if (buttons_.find(request->pin) != buttons_.end()) { - response->success = false; - return; - } - - Button button; - button.pin = request->pin; - button.name = request->name; - button.publisher = create_publisher("button/" + request->name, 10); - - buttons_.insert({request->pin, button}); - - pinMode(request->pin, INPUT); - pullUpDnControl(request->pin, PUD_UP); - - response->success = true; - } - - void ButtonGpioController::check_button(const std::shared_ptr request, std::shared_ptr response) { - if (buttons_.find(request->pin) == buttons_.end()) { - response->status = false; - return; - } - - response->status = digitalRead(request->pin) == LOW; - } -} - -int main(int argc, char** argv) { - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} +#include + +namespace Modelec { + ButtonGpioController::ButtonGpioController() : Node("button_gpio_controller") { + if (wiringPiSetupGpio() == -1) + { + RCLCPP_ERROR(this->get_logger(), "Failed to initialize GPIO."); + rclcpp::shutdown(); + } + + new_button_service_ = create_service("add_button", std::bind(&ButtonGpioController::new_button, this, std::placeholders::_1, std::placeholders::_2)); + button_server_ = create_service("button", std::bind(&ButtonGpioController::check_button, this, std::placeholders::_1, std::placeholders::_2)); + timer_ = create_wall_timer(std::chrono::seconds(1), [this]() { + for (auto& button : buttons_) { + modelec_interfaces::msg::Button msg; + msg.pin = button.second.pin; + msg.state = digitalRead(button.second.pin) == LOW; + button.second.publisher->publish(msg); + } + }); + } + + void ButtonGpioController::new_button(const std::shared_ptr request, std::shared_ptr response) { + if (buttons_.find(request->pin) != buttons_.end()) { + response->success = false; + return; + } + + Button button; + button.pin = request->pin; + button.name = request->name; + button.publisher = create_publisher("button/" + request->name, 10); + + buttons_.insert({request->pin, button}); + + pinMode(request->pin, INPUT); + pullUpDnControl(request->pin, PUD_UP); + + response->success = true; + } + + void ButtonGpioController::check_button(const std::shared_ptr request, std::shared_ptr response) { + if (buttons_.find(request->pin) == buttons_.end()) { + response->status = false; + return; + } + + response->status = digitalRead(request->pin) == LOW; + } +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/src/modelec/src/gamecontroller_listener.cpp b/src/modelec_core/src/gamecontroller_listener.cpp similarity index 93% rename from src/modelec/src/gamecontroller_listener.cpp rename to src/modelec_core/src/gamecontroller_listener.cpp index 7aacbae..dc50903 100644 --- a/src/modelec/src/gamecontroller_listener.cpp +++ b/src/modelec_core/src/gamecontroller_listener.cpp @@ -1,6 +1,6 @@ #include "modelec/gamecontroller_listener.hpp" -#include "modelec/utils.hpp" -#include +#include "modelec_utils/utils.hpp" +#include namespace Modelec { @@ -14,11 +14,11 @@ namespace Modelec servo_publisher_ = this->create_publisher("arm_control", 10); // Service to create a new serial listener - auto request = std::make_shared(); + auto request = std::make_shared(); request->name = "odometry"; request->bauds = 115200; request->serial_port = "/dev/ttyACM0"; - auto client = this->create_client("add_serial_listener"); + auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) @@ -42,7 +42,7 @@ namespace Modelec clear_pca_publisher_ = this->create_publisher("clear_pca9685", 10); - pca9685_publisher_ = this->create_publisher( + pca9685_publisher_ = this->create_publisher( "servo_control", 10); } else @@ -60,7 +60,7 @@ namespace Modelec RCLCPP_ERROR(this->get_logger(), "Service call failed"); } - client_ = this->create_client("add_servo"); + client_ = this->create_client("add_servo"); // ensure the server is up while (!client_->wait_for_service(std::chrono::seconds(1))) @@ -75,7 +75,7 @@ namespace Modelec for (auto servo : solarPannelServos) { - auto request = std::make_shared(); + auto request = std::make_shared(); request->pin = servo.pin; auto res = client_->async_send_request(request); if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) == @@ -280,7 +280,7 @@ namespace Modelec int solarPannelAngle = static_cast(Modelec::mapValue(static_cast(msg->axes[2]), -1.0f, 1.0f, solarPannelServos[0].startAngle, solarPannelServos[0].endAngle)); - auto solarPannelAngleMessage = modelec_interface::msg::PCA9685Servo(); + auto solarPannelAngleMessage = modelec_interfaces::msg::PCA9685Servo(); solarPannelAngleMessage.pin = solarPannelServos[0].pin; solarPannelAngleMessage.angle = solarPannelAngle; pca9685_publisher_->publish(solarPannelAngleMessage); @@ -292,7 +292,7 @@ namespace Modelec int solarPannelAngle = static_cast(Modelec::mapValue(static_cast(msg->axes[5]), -1.0f, 1.0f, solarPannelServos[1].startAngle, solarPannelServos[1].endAngle)); - auto solarPannelAngleMessage = modelec_interface::msg::PCA9685Servo(); + auto solarPannelAngleMessage = modelec_interfaces::msg::PCA9685Servo(); solarPannelAngleMessage.pin = solarPannelServos[1].pin; solarPannelAngleMessage.angle = solarPannelAngle; pca9685_publisher_->publish(solarPannelAngleMessage); diff --git a/src/modelec/src/move_controller.cpp b/src/modelec_core/src/move_controller.cpp similarity index 100% rename from src/modelec/src/move_controller.cpp rename to src/modelec_core/src/move_controller.cpp diff --git a/src/modelec/src/solenoid_controller.cpp b/src/modelec_core/src/solenoid_controller.cpp similarity index 87% rename from src/modelec/src/solenoid_controller.cpp rename to src/modelec_core/src/solenoid_controller.cpp index 941bc99..2adce4d 100644 --- a/src/modelec/src/solenoid_controller.cpp +++ b/src/modelec_core/src/solenoid_controller.cpp @@ -5,22 +5,22 @@ namespace Modelec { SolenoidController::SolenoidController() : Node("solenoid_controller") { wiringPiSetup(); - solenoid_subscriber_ = this->create_subscription( + solenoid_subscriber_ = this->create_subscription( "solenoid", 10, std::bind(&SolenoidController::activateSolenoid, this, std::placeholders::_1)); - add_solenoid_service_ = this->create_service( + add_solenoid_service_ = this->create_service( "add_solenoid", std::bind(&SolenoidController::addSolenoid, this, std::placeholders::_1, std::placeholders::_2)); } - void SolenoidController::activateSolenoid(const modelec_interface::msg::Solenoid::SharedPtr msg) { + void SolenoidController::activateSolenoid(const modelec_interfaces::msg::Solenoid::SharedPtr msg) { // Activate solenoid if (solenoid_pin_.find(msg->pin) != solenoid_pin_.end()) { digitalWrite(msg->pin, msg->state ? HIGH : LOW); } } - void SolenoidController::addSolenoid(const std::shared_ptr request, - std::shared_ptr response) { + void SolenoidController::addSolenoid(const std::shared_ptr request, + std::shared_ptr response) { if (solenoid_pin_.find(request->pin) != solenoid_pin_.end()) { response->success = false; return; diff --git a/src/modelec/src/speed_result.cpp b/src/modelec_core/src/speed_result.cpp similarity index 83% rename from src/modelec/src/speed_result.cpp rename to src/modelec_core/src/speed_result.cpp index 6780a97..508d4a5 100644 --- a/src/modelec/src/speed_result.cpp +++ b/src/modelec_core/src/speed_result.cpp @@ -7,7 +7,7 @@ namespace Modelec { std::cerr << "Error opening file: " << fileName_ << std::endl; } - sub_speed_ = this->create_subscription( + sub_speed_ = this->create_subscription( "odometry/speed", 10, std::bind(&SpeedResultNode::SpeedCallback, this, std::placeholders::_1)); @@ -20,7 +20,7 @@ namespace Modelec { file_.close(); } - void SpeedResultNode::SpeedCallback(const modelec_interface::msg::OdometrySpeed::SharedPtr msg) + void SpeedResultNode::SpeedCallback(const modelec_interfaces::msg::OdometrySpeed::SharedPtr msg) { std::string time = std::to_string(this->now().nanoseconds() - start_time_.nanoseconds()); file_ << time << "," diff --git a/src/modelec/worlds/empty.world b/src/modelec_core/worlds/empty.world similarity index 100% rename from src/modelec/worlds/empty.world rename to src/modelec_core/worlds/empty.world diff --git a/src/modelec/worlds/gazebo.world b/src/modelec_core/worlds/gazebo.world similarity index 100% rename from src/modelec/worlds/gazebo.world rename to src/modelec_core/worlds/gazebo.world diff --git a/src/modelec/worlds/obstacles.world b/src/modelec_core/worlds/obstacles.world similarity index 100% rename from src/modelec/worlds/obstacles.world rename to src/modelec_core/worlds/obstacles.world diff --git a/src/modelec_gui/CMakeLists.txt b/src/modelec_gui/CMakeLists.txt index b077db1..90172f6 100644 --- a/src/modelec_gui/CMakeLists.txt +++ b/src/modelec_gui/CMakeLists.txt @@ -12,7 +12,7 @@ set(CMAKE_AUTOMOC ON) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) -find_package(modelec_interface REQUIRED) +find_package(modelec_interfaces REQUIRED) find_package(Qt6 COMPONENTS Core Gui @@ -33,7 +33,7 @@ add_executable(modelec_gui ament_target_dependencies(modelec_gui rclcpp std_msgs - modelec_interface + modelec_interfaces ) target_link_libraries(modelec_gui Qt6::Core diff --git a/src/modelec_gui/include/modelec_gui/pages/test_page.hpp b/src/modelec_gui/include/modelec_gui/pages/test_page.hpp index 6e596fe..1c15e69 100644 --- a/src/modelec_gui/include/modelec_gui/pages/test_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/test_page.hpp @@ -7,12 +7,12 @@ #include -#include -#include -#include -#include -#include -#include +#include +#include +#include +#include +#include +#include namespace ModelecGUI { @@ -45,17 +45,17 @@ namespace ModelecGUI { rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr sub_; + rclcpp::Subscription::SharedPtr sub_; - rclcpp::Publisher::SharedPtr pub_add_waypoint_; + rclcpp::Publisher::SharedPtr pub_add_waypoint_; // client - rclcpp::Client::SharedPtr client_; - rclcpp::Client::SharedPtr client_start_; - rclcpp::Client::SharedPtr client_get_pid_; - rclcpp::Client::SharedPtr client_set_pid_; + rclcpp::Client::SharedPtr client_; + rclcpp::Client::SharedPtr client_start_; + rclcpp::Client::SharedPtr client_get_pid_; + rclcpp::Client::SharedPtr client_set_pid_; - void PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg); + void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); }; } // namespace Modelec \ No newline at end of file diff --git a/src/modelec_gui/package.xml b/src/modelec_gui/package.xml index 710723f..1e190b4 100644 --- a/src/modelec_gui/package.xml +++ b/src/modelec_gui/package.xml @@ -9,7 +9,7 @@ ament_cmake - modelec_interface + modelec_interfaces rclcpp std_msgs diff --git a/src/modelec_gui/src/pages/test_page.cpp b/src/modelec_gui/src/pages/test_page.cpp index 51a50af..9de30e3 100644 --- a/src/modelec_gui/src/pages/test_page.cpp +++ b/src/modelec_gui/src/pages/test_page.cpp @@ -11,9 +11,9 @@ namespace ModelecGUI 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(); + auto request = std::make_shared(); request->start = true; - client_start_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { + client_start_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { if (response.get()->success) { RCLCPP_INFO(node_->get_logger(), "Start command sent successfully."); @@ -42,10 +42,10 @@ namespace ModelecGUI askPID_ = new QPushButton("Ask PID"); connect(askPID_, &QPushButton::clicked, this, [this]() { // Create a request for the PID service - auto request = std::make_shared(); + auto request = std::make_shared(); // Make the asynchronous service call - client_get_pid_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { + client_get_pid_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { if (auto res = response.get()) { QMetaObject::invokeMethod(this, [this, res]() { pPIDBox_->setText(QString("%1").arg(res->p)); @@ -69,13 +69,13 @@ namespace ModelecGUI setPID_ = new QPushButton("Set PID"); connect(setPID_, &QPushButton::clicked, this, [this]() { // Create a request for the PID service - auto request = std::make_shared(); + auto request = std::make_shared(); request->p = pPIDBox_->text().toFloat(); request->i = iPIDBox_->text().toFloat(); request->d = dPIDBox_->text().toFloat(); // Make the asynchronous service call - client_set_pid_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { + client_set_pid_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { if (response.get()->success) { RCLCPP_INFO(node_->get_logger(), "Set PID command sent successfully."); @@ -90,10 +90,10 @@ namespace ModelecGUI askSpeed_ = new QPushButton("Ask speed"); connect(askSpeed_, &QPushButton::clicked, this, [this]() { // Create a request for the speed service - auto request = std::make_shared(); + auto request = std::make_shared(); // Make the asynchronous service call - client_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { + client_->async_send_request(request, [this](rclcpp::Client::SharedFuture response) { if (auto res = response.get()) { QMetaObject::invokeMethod(this, [this, res]() { xSpeedBox_->setText(QString("x: %1").arg(res->x)); @@ -120,7 +120,7 @@ namespace ModelecGUI startTest_ = new QPushButton("Start Test"); connect(startTest_, &QPushButton::clicked, this, [this]() { - auto firstRequest = std::make_shared(); + auto firstRequest = std::make_shared(); firstRequest->id = 0; firstRequest->is_end = false; firstRequest->x = 100.0; @@ -129,7 +129,7 @@ namespace ModelecGUI pub_add_waypoint_->publish(*firstRequest); - auto secondRequest = std::make_shared(); + auto secondRequest = std::make_shared(); secondRequest->id = 1; secondRequest->is_end = true; secondRequest->x = 0.0; @@ -151,15 +151,15 @@ namespace ModelecGUI setLayout(mainLayout_); // Set up subscription - sub_ = node_->create_subscription( + sub_ = node_->create_subscription( "/odometry/get_position", 10, std::bind(&TestPage::PositionCallback, this, std::placeholders::_1)); - pub_add_waypoint_ = node_->create_publisher( + pub_add_waypoint_ = node_->create_publisher( "/odometry/add_waypoint", 10); // Set up service client - client_ = node_->create_client("odometry/speed"); + client_ = node_->create_client("odometry/speed"); // Wait for the service to be available while (!client_->wait_for_service(std::chrono::seconds(1))) { @@ -170,7 +170,7 @@ namespace ModelecGUI RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again..."); } - client_start_ = node_->create_client("odometry/start"); + client_start_ = node_->create_client("odometry/start"); while (!client_start_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { @@ -180,7 +180,7 @@ namespace ModelecGUI RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again..."); } - client_get_pid_ = node_->create_client("odometry/get_pid"); + client_get_pid_ = node_->create_client("odometry/get_pid"); while (!client_get_pid_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) { @@ -190,7 +190,7 @@ namespace ModelecGUI RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again..."); } - client_set_pid_ = node_->create_client("odometry/set_pid"); + client_set_pid_ = node_->create_client("odometry/set_pid"); 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."); @@ -202,7 +202,7 @@ namespace ModelecGUI TestPage::~TestPage() = default; - void TestPage::PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg) + void TestPage::PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { QMetaObject::invokeMethod(this, [this, msg]() { xBox_->setText(QString("x: %1").arg(msg->x)); diff --git a/src/modelec_interface/CMakeLists.txt b/src/modelec_interfaces/CMakeLists.txt similarity index 98% rename from src/modelec_interface/CMakeLists.txt rename to src/modelec_interfaces/CMakeLists.txt index edbec23..d66853b 100644 --- a/src/modelec_interface/CMakeLists.txt +++ b/src/modelec_interfaces/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.8) -project(modelec_interface) +project(modelec_interfaces) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) diff --git a/src/modelec_interface/LICENSE b/src/modelec_interfaces/LICENSE similarity index 100% rename from src/modelec_interface/LICENSE rename to src/modelec_interfaces/LICENSE diff --git a/src/modelec_interface/msg/Alim/AlimEmg.msg b/src/modelec_interfaces/msg/Alim/AlimEmg.msg similarity index 100% rename from src/modelec_interface/msg/Alim/AlimEmg.msg rename to src/modelec_interfaces/msg/Alim/AlimEmg.msg diff --git a/src/modelec_interface/msg/Button.msg b/src/modelec_interfaces/msg/Button.msg similarity index 100% rename from src/modelec_interface/msg/Button.msg rename to src/modelec_interfaces/msg/Button.msg diff --git a/src/modelec_interface/msg/Odometry/OdometryAddWaypoint.msg b/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg similarity index 100% rename from src/modelec_interface/msg/Odometry/OdometryAddWaypoint.msg rename to src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg diff --git a/src/modelec_interface/msg/Odometry/OdometryPos.msg b/src/modelec_interfaces/msg/Odometry/OdometryPos.msg similarity index 100% rename from src/modelec_interface/msg/Odometry/OdometryPos.msg rename to src/modelec_interfaces/msg/Odometry/OdometryPos.msg diff --git a/src/modelec_interface/msg/Odometry/OdometrySpeed.msg b/src/modelec_interfaces/msg/Odometry/OdometrySpeed.msg similarity index 100% rename from src/modelec_interface/msg/Odometry/OdometrySpeed.msg rename to src/modelec_interfaces/msg/Odometry/OdometrySpeed.msg diff --git a/src/modelec_interface/msg/Odometry/OdometryStart.msg b/src/modelec_interfaces/msg/Odometry/OdometryStart.msg similarity index 100% rename from src/modelec_interface/msg/Odometry/OdometryStart.msg rename to src/modelec_interfaces/msg/Odometry/OdometryStart.msg diff --git a/src/modelec_interface/msg/Odometry/OdometryToF.msg b/src/modelec_interfaces/msg/Odometry/OdometryToF.msg similarity index 100% rename from src/modelec_interface/msg/Odometry/OdometryToF.msg rename to src/modelec_interfaces/msg/Odometry/OdometryToF.msg diff --git a/src/modelec_interface/msg/Odometry/OdometryWaypointReach.msg b/src/modelec_interfaces/msg/Odometry/OdometryWaypointReach.msg similarity index 100% rename from src/modelec_interface/msg/Odometry/OdometryWaypointReach.msg rename to src/modelec_interfaces/msg/Odometry/OdometryWaypointReach.msg diff --git a/src/modelec_interface/msg/Odometry/PID/OdometryPid.msg b/src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg similarity index 100% rename from src/modelec_interface/msg/Odometry/PID/OdometryPid.msg rename to src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg diff --git a/src/modelec_interface/msg/PCA9685Servo.msg b/src/modelec_interfaces/msg/PCA9685Servo.msg similarity index 100% rename from src/modelec_interface/msg/PCA9685Servo.msg rename to src/modelec_interfaces/msg/PCA9685Servo.msg diff --git a/src/modelec_interface/msg/ServoMode.msg b/src/modelec_interfaces/msg/ServoMode.msg similarity index 100% rename from src/modelec_interface/msg/ServoMode.msg rename to src/modelec_interfaces/msg/ServoMode.msg diff --git a/src/modelec_interface/msg/Solenoid.msg b/src/modelec_interfaces/msg/Solenoid.msg similarity index 100% rename from src/modelec_interface/msg/Solenoid.msg rename to src/modelec_interfaces/msg/Solenoid.msg diff --git a/src/modelec_interface/package.xml b/src/modelec_interfaces/package.xml similarity index 95% rename from src/modelec_interface/package.xml rename to src/modelec_interfaces/package.xml index 3f8059b..9d861e7 100644 --- a/src/modelec_interface/package.xml +++ b/src/modelec_interfaces/package.xml @@ -1,7 +1,7 @@ - modelec_interface + modelec_interfaces 0.0.0 TODO: Package description modelec diff --git a/src/modelec_interface/srv/AddButton.srv b/src/modelec_interfaces/srv/AddButton.srv similarity index 100% rename from src/modelec_interface/srv/AddButton.srv rename to src/modelec_interfaces/srv/AddButton.srv diff --git a/src/modelec_interface/srv/AddSerialListener.srv b/src/modelec_interfaces/srv/AddSerialListener.srv similarity index 100% rename from src/modelec_interface/srv/AddSerialListener.srv rename to src/modelec_interfaces/srv/AddSerialListener.srv diff --git a/src/modelec_interface/srv/AddServoMotor.srv b/src/modelec_interfaces/srv/AddServoMotor.srv similarity index 100% rename from src/modelec_interface/srv/AddServoMotor.srv rename to src/modelec_interfaces/srv/AddServoMotor.srv diff --git a/src/modelec_interface/srv/AddSolenoid.srv b/src/modelec_interfaces/srv/AddSolenoid.srv similarity index 100% rename from src/modelec_interface/srv/AddSolenoid.srv rename to src/modelec_interfaces/srv/AddSolenoid.srv diff --git a/src/modelec_interface/srv/Alim/AlimBau.srv b/src/modelec_interfaces/srv/Alim/AlimBau.srv similarity index 100% rename from src/modelec_interface/srv/Alim/AlimBau.srv rename to src/modelec_interfaces/srv/Alim/AlimBau.srv diff --git a/src/modelec_interface/srv/Alim/AlimEmg.srv b/src/modelec_interfaces/srv/Alim/AlimEmg.srv similarity index 100% rename from src/modelec_interface/srv/Alim/AlimEmg.srv rename to src/modelec_interfaces/srv/Alim/AlimEmg.srv diff --git a/src/modelec_interface/srv/Alim/AlimIn.srv b/src/modelec_interfaces/srv/Alim/AlimIn.srv similarity index 100% rename from src/modelec_interface/srv/Alim/AlimIn.srv rename to src/modelec_interfaces/srv/Alim/AlimIn.srv diff --git a/src/modelec_interface/srv/Alim/AlimOut.srv b/src/modelec_interfaces/srv/Alim/AlimOut.srv similarity index 100% rename from src/modelec_interface/srv/Alim/AlimOut.srv rename to src/modelec_interfaces/srv/Alim/AlimOut.srv diff --git a/src/modelec_interface/srv/Alim/AlimTemp.srv b/src/modelec_interfaces/srv/Alim/AlimTemp.srv similarity index 100% rename from src/modelec_interface/srv/Alim/AlimTemp.srv rename to src/modelec_interfaces/srv/Alim/AlimTemp.srv diff --git a/src/modelec_interface/srv/Button.srv b/src/modelec_interfaces/srv/Button.srv similarity index 100% rename from src/modelec_interface/srv/Button.srv rename to src/modelec_interfaces/srv/Button.srv diff --git a/src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv b/src/modelec_interfaces/srv/Odometry/OdometryAddWaypoint.srv similarity index 100% rename from src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv rename to src/modelec_interfaces/srv/Odometry/OdometryAddWaypoint.srv diff --git a/src/modelec_interface/srv/Odometry/OdometryPosition.srv b/src/modelec_interfaces/srv/Odometry/OdometryPosition.srv similarity index 100% rename from src/modelec_interface/srv/Odometry/OdometryPosition.srv rename to src/modelec_interfaces/srv/Odometry/OdometryPosition.srv diff --git a/src/modelec_interface/srv/Odometry/OdometrySpeed.srv b/src/modelec_interfaces/srv/Odometry/OdometrySpeed.srv similarity index 100% rename from src/modelec_interface/srv/Odometry/OdometrySpeed.srv rename to src/modelec_interfaces/srv/Odometry/OdometrySpeed.srv diff --git a/src/modelec_interface/srv/Odometry/OdometryStart.srv b/src/modelec_interfaces/srv/Odometry/OdometryStart.srv similarity index 100% rename from src/modelec_interface/srv/Odometry/OdometryStart.srv rename to src/modelec_interfaces/srv/Odometry/OdometryStart.srv diff --git a/src/modelec_interface/srv/Odometry/OdometryToF.srv b/src/modelec_interfaces/srv/Odometry/OdometryToF.srv similarity index 100% rename from src/modelec_interface/srv/Odometry/OdometryToF.srv rename to src/modelec_interfaces/srv/Odometry/OdometryToF.srv diff --git a/src/modelec_interface/srv/Odometry/PID/OdometryGetPid.srv b/src/modelec_interfaces/srv/Odometry/PID/OdometryGetPid.srv similarity index 100% rename from src/modelec_interface/srv/Odometry/PID/OdometryGetPid.srv rename to src/modelec_interfaces/srv/Odometry/PID/OdometryGetPid.srv diff --git a/src/modelec_interface/srv/Odometry/PID/OdometrySetPid.srv b/src/modelec_interfaces/srv/Odometry/PID/OdometrySetPid.srv similarity index 100% rename from src/modelec_interface/srv/Odometry/PID/OdometrySetPid.srv rename to src/modelec_interfaces/srv/Odometry/PID/OdometrySetPid.srv diff --git a/src/modelec_interface/srv/Tirette.srv b/src/modelec_interfaces/srv/Tirette.srv similarity index 100% rename from src/modelec_interface/srv/Tirette.srv rename to src/modelec_interfaces/srv/Tirette.srv diff --git a/src/modelec/include/modelec/lidar_controller.hpp b/src/modelec_sensors/include/modelec_sensors/lidar_controller.hpp similarity index 100% rename from src/modelec/include/modelec/lidar_controller.hpp rename to src/modelec_sensors/include/modelec_sensors/lidar_controller.hpp diff --git a/src/modelec/include/modelec/tirette_controller.hpp b/src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp similarity index 85% rename from src/modelec/include/modelec/tirette_controller.hpp rename to src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp index 7032bfc..1c0835d 100644 --- a/src/modelec/include/modelec/tirette_controller.hpp +++ b/src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp @@ -1,14 +1,14 @@ #pragma once #include -#include +#include #include #include #define REFRESH_RATE 100 #define GPIO_TIRETTE 17 -using TiretteInterface = modelec_interface::srv::Tirette; +using TiretteInterface = modelec_interfaces::srv::Tirette; namespace Modelec { class TiretteController : public rclcpp::Node { diff --git a/src/modelec/src/lidar_controller.cpp b/src/modelec_sensors/src/lidar_controller.cpp similarity index 93% rename from src/modelec/src/lidar_controller.cpp rename to src/modelec_sensors/src/lidar_controller.cpp index 63a643b..c74b927 100644 --- a/src/modelec/src/lidar_controller.cpp +++ b/src/modelec_sensors/src/lidar_controller.cpp @@ -1,4 +1,4 @@ -#include +#include namespace Modelec { LidarController::LidarController() : Node("lidar_controller") { diff --git a/src/modelec/src/tirette_controller.cpp b/src/modelec_sensors/src/tirette_controller.cpp similarity index 92% rename from src/modelec/src/tirette_controller.cpp rename to src/modelec_sensors/src/tirette_controller.cpp index 64c903f..bd97b92 100644 --- a/src/modelec/src/tirette_controller.cpp +++ b/src/modelec_sensors/src/tirette_controller.cpp @@ -1,4 +1,4 @@ -#include +#include #include namespace Modelec { @@ -17,7 +17,7 @@ namespace Modelec { tirette_state = false; // Initialize the service - service = this->create_service( + service = this->create_service( "tirette", std::bind(&TiretteController::check_tirette, this, std::placeholders::_1, std::placeholders::_2)); diff --git a/src/modelec_utils/include/modelec_utils/utils.hpp b/src/modelec_utils/include/modelec_utils/utils.hpp new file mode 100644 index 0000000..44657e2 --- /dev/null +++ b/src/modelec_utils/include/modelec_utils/utils.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +namespace Modelec { + #define PI 3.14159265358979323846 + + std::vector split(const std::string &s, char delim); + + std::string join(const std::vector &v, const std::string &delim); + + bool startsWith(const std::string &s, const std::string &prefix); + + bool endsWith(const std::string &s, const std::string &suffix); + + bool contains(const std::string &s, const std::string &substring); + + template ::value, int>::type = 0> + T mapValue(T v, T v_min, T v_max, T v_min_prime, T v_max_prime) { + return v_min_prime + (((v - v_min) * (v_max_prime - v_min_prime)) / (v_max - v_min)); + } +} diff --git a/src/modelec_utils/src/utils.cpp b/src/modelec_utils/src/utils.cpp new file mode 100644 index 0000000..b3824b8 --- /dev/null +++ b/src/modelec_utils/src/utils.cpp @@ -0,0 +1,41 @@ +#include + +namespace Modelec { + + std::vector split(const std::string &s, char delim) { + std::vector result; + std::string token; + std::istringstream tokenStream(s); + while (std::getline(tokenStream, token, delim)) { + result.push_back(token); + } + return result; + } + + std::string join(const std::vector& v, const std::string& delim) + { + std::string result; + for (size_t i = 0; i < v.size(); ++i) { + if (i != 0) { + result += delim; + } + result += v[i]; + } + return result; + } + + bool startsWith(const std::string& s, const std::string& prefix) + { + return s.rfind(prefix, 0) == 0; + } + + bool endsWith(const std::string& s, const std::string& suffix) + { + return s.size() >= suffix.size() && s.rfind(suffix) == s.size() - suffix.size(); + } + + bool contains(const std::string& s, const std::string& substring) + { + return s.find(substring) != std::string::npos; + } +}; \ No newline at end of file