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

refactor everything in different pkg

Browse Source
This commit is contained in:
acki
2025-04-22 01:57:11 -04:00
parent 575fbfee79
commit 6953a1c95d
86 changed files with 600 additions and 635 deletions
Download Patch File Download Diff File Expand all files Collapse all files

35
src/modelec/include/modelec/button_gpio_controller.hpp
View File

@@ -1,35 +0,0 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <modelec_interface/srv/add_button.hpp>
#include <modelec_interface/srv/button.hpp>
#include <modelec_interface/msg/button.hpp>
#include <wiringPi.h>
namespace Modelec {
class ButtonGpioController : public rclcpp::Node {
struct Button {
int pin;
rclcpp::Publisher<modelec_interface::msg::Button>::SharedPtr publisher;
std::string name;
};
public:
ButtonGpioController();
private:
// service
rclcpp::Service<modelec_interface::srv::AddButton>::SharedPtr new_button_service_;
rclcpp::Service<modelec_interface::srv::Button>::SharedPtr button_server_;
// service callbacks
void new_button(const std::shared_ptr<modelec_interface::srv::AddButton::Request> request, std::shared_ptr<modelec_interface::srv::AddButton::Response> response);
void check_button(const std::shared_ptr<modelec_interface::srv::Button::Request> request, std::shared_ptr<modelec_interface::srv::Button::Response> response);
// timer
rclcpp::TimerBase::SharedPtr timer_;
std::map<int, Button> buttons_;
};
}

157
src/modelec/include/modelec/pcb_odo_interface.hpp
View File

@@ -1,157 +0,0 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <queue>
#include <mutex>
#include <future>
#include <std_msgs/msg/string.hpp>
#include <modelec_interface/msg/odometry_pos.hpp>
#include <modelec_interface/msg/odometry_speed.hpp>
#include <modelec_interface/msg/odometry_to_f.hpp>
#include <modelec_interface/msg/odometry_waypoint_reach.hpp>
#include <modelec_interface/msg/odometry_add_waypoint.hpp>
#include <modelec_interface/msg/odometry_start.hpp>
#include <modelec_interface/msg/odometry_pid.hpp>
#include <modelec_interface/srv/odometry_position.hpp>
#include <modelec_interface/srv/odometry_speed.hpp>
#include <modelec_interface/srv/odometry_to_f.hpp>
#include <modelec_interface/srv/add_serial_listener.hpp>
#include <modelec_interface/srv/odometry_start.hpp>
#include <modelec_interface/srv/odometry_get_pid.hpp>
#include <modelec_interface/srv/odometry_set_pid.hpp>
#include <modelec_interface/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;
long 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_interface::msg::OdometryPos>::SharedPtr odo_pos_publisher_;
rclcpp::Publisher<modelec_interface::msg::OdometrySpeed>::SharedPtr odo_speed_publisher_;
rclcpp::Publisher<modelec_interface::msg::OdometryToF>::SharedPtr odo_tof_publisher_;
rclcpp::Publisher<modelec_interface::msg::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_publisher_;
rclcpp::Publisher<modelec_interface::msg::OdometryPid>::SharedPtr odo_pid_publisher_;
rclcpp::Subscription<modelec_interface::msg::OdometryAddWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
rclcpp::Subscription<modelec_interface::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
rclcpp::Subscription<modelec_interface::msg::OdometryPid>::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<modelec_interface::srv::OdometryToF>::SharedPtr get_tof_service_;
rclcpp::Service<modelec_interface::srv::OdometrySpeed>::SharedPtr get_speed_service_;
rclcpp::Service<modelec_interface::srv::OdometryPosition>::SharedPtr get_position_service_;
rclcpp::Service<modelec_interface::srv::OdometryStart>::SharedPtr set_start_service_;
rclcpp::Service<modelec_interface::srv::OdometryGetPid>::SharedPtr get_pid_service_;
rclcpp::Service<modelec_interface::srv::OdometrySetPid>::SharedPtr set_pid_service_;
rclcpp::Service<modelec_interface::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_interface::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response);
void HandleGetSpeed(const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request> request,
std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response);
void HandleGetPosition(const std::shared_ptr<modelec_interface::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryPosition::Response> response);
void HandleGetStart(const std::shared_ptr<modelec_interface::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryStart::Response> response);
void HandleGetPID(const std::shared_ptr<modelec_interface::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryGetPid::Response> response);
void HandleSetPID(const std::shared_ptr<modelec_interface::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interface::srv::OdometrySetPid::Response> response);
void HandleAddWaypoint(const std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interface::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);
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_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

24
src/modelec/include/modelec/solenoid_controller.hpp
View File

@@ -1,24 +0,0 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <unordered_set>
#include <wiringPi.h>
#include <modelec_interface/msg/solenoid.hpp>
#include <modelec_interface/srv/add_solenoid.hpp>
namespace Modelec {
class SolenoidController : public rclcpp::Node {
public:
SolenoidController();
private:
std::unordered_set<int> solenoid_pin_;
rclcpp::Subscription<modelec_interface::msg::Solenoid>::SharedPtr solenoid_subscriber_;
rclcpp::Service<modelec_interface::srv::AddSolenoid>::SharedPtr add_solenoid_service_;
void activateSolenoid(const modelec_interface::msg::Solenoid::SharedPtr msg);
void addSolenoid(const std::shared_ptr<modelec_interface::srv::AddSolenoid::Request> request, std::shared_ptr<modelec_interface::srv::AddSolenoid::Response> response);
};
}

47
src/modelec/include/modelec/utils.hpp
View File

@@ -1,47 +0,0 @@
#pragma once
#include <string>
#include <vector>
#include <sstream>
namespace Modelec {
#define PI 3.14159265358979323846
inline std::vector<std::string> split(const std::string &s, char delim) {
std::vector<std::string> 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<std::string> &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 <typename T, typename std::enable_if<std::is_arithmetic<T>::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));
}
}

9
src/modelec/include/modelec/multiple_serial_listener.hpp → src/modelec_com/include/modelec_com/multiple_serial_listener.hpp
View File

@@ -3,8 +3,7 @@
#include <rclcpp/rclcpp.hpp>
#include <boost/asio.hpp>
#include <std_msgs/msg/string.hpp>
#include <modelec_interface/srv/add_serial_listener.hpp>
#include <modelec_interface/msg/write_serial.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <deque>
#include <thread>
#include <mutex>
@@ -60,10 +59,10 @@ private:
boost::asio::io_service io;
rclcpp::Service<modelec_interface::srv::AddSerialListener>::SharedPtr add_serial_listener_service_;
rclcpp::Service<modelec_interfaces::srv::AddSerialListener>::SharedPtr add_serial_listener_service_;
void add_serial_listener(
const std::shared_ptr<modelec_interface::srv::AddSerialListener::Request> request,
std::shared_ptr<modelec_interface::srv::AddSerialListener::Response> response);
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);

8
src/modelec/include/modelec/pca9685_controller.hpp → src/modelec_com/include/modelec_com/pca9685_controller.hpp
View File

@@ -5,8 +5,8 @@
#include <wiringPi.h>
#include <wiringPiI2C.h>
#include <modelec_interface/msg/pca9685_servo.hpp>
#include <modelec_interface/srv/add_servo_motor.hpp>
#include <modelec_interfaces/msg/pca9685_servo.hpp>
#include <modelec_interfaces/srv/add_servo_motor.hpp>
#include <std_msgs/msg/empty.hpp>
#define PCA9685_FREQUENCY 25000000.0
@@ -24,14 +24,14 @@ namespace Modelec {
private:
int fd; // File descriptor for I2C communication
rclcpp::Subscription<modelec_interface::msg::PCA9685Servo>::SharedPtr servo_subscriber_;
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)
std::unordered_set<int> active_servos;
// service to add a servo
rclcpp::Service<modelec_interface::srv::AddServoMotor>::SharedPtr add_servo_service_;
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);

50
src/modelec/include/modelec/pcb_alim_interface.hpp → src/modelec_com/include/modelec_com/pcb_alim_interface.hpp
View File

@@ -2,12 +2,12 @@
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <modelec_interface/srv/alim_out.hpp>
#include <modelec_interface/srv/alim_in.hpp>
#include <modelec_interface/srv/alim_bau.hpp>
#include <modelec_interface/srv/alim_emg.hpp>
#include <modelec_interface/srv/alim_temp.hpp>
#include <modelec_interface/msg/alim_emg.hpp>
#include <modelec_interfaces/srv/alim_out.hpp>
#include <modelec_interfaces/srv/alim_in.hpp>
#include <modelec_interfaces/srv/alim_bau.hpp>
#include <modelec_interfaces/srv/alim_emg.hpp>
#include <modelec_interfaces/srv/alim_temp.hpp>
#include <modelec_interfaces/msg/alim_emg.hpp>
namespace Modelec
{
@@ -39,15 +39,15 @@ private:
void PCBCallback(const std_msgs::msg::String::SharedPtr msg);
rclcpp::Subscription<modelec_interface::msg::AlimEmg>::SharedPtr pcb_emg_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::AlimEmg>::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<modelec_interface::srv::AlimOut>::SharedPtr pcb_out_service_;
rclcpp::Service<modelec_interface::srv::AlimIn>::SharedPtr pcb_in_service_;
rclcpp::Service<modelec_interface::srv::AlimBau>::SharedPtr pcb_bau_service_;
rclcpp::Service<modelec_interface::srv::AlimEmg>::SharedPtr pcb_emg_service_;
rclcpp::Service<modelec_interface::srv::AlimTemp>::SharedPtr pcb_temp_service_;
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_;
@@ -65,28 +65,28 @@ private:
std::mutex pcb_temp_mutex_;
void HandleGetPCBOutData(
const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interface::srv::AlimOut::Response> response);
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_interface::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interface::srv::AlimOut::Response> response);
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_interface::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interface::srv::AlimIn::Response> response);
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_interface::srv::AlimBau::Request> request,
std::shared_ptr<modelec_interface::srv::AlimBau::Response> response);
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_interface::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interface::srv::AlimEmg::Response> response);
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_interface::srv::AlimTemp::Request> request,
std::shared_ptr<modelec_interface::srv::AlimTemp::Response> response);
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);

158
src/modelec_com/include/modelec_com/pcb_odo_interface.hpp Normal file
View File

@@ -0,0 +1,158 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <queue>
#include <mutex>
#include <future>
#include <std_msgs/msg/string.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_speed.hpp>
#include <modelec_interfaces/msg/odometry_to_f.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_start.hpp>
#include <modelec_interfaces/msg/odometry_pid.hpp>
#include <modelec_interfaces/srv/odometry_position.hpp>
#include <modelec_interfaces/srv/odometry_speed.hpp>
#include <modelec_interfaces/srv/odometry_to_f.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <modelec_interfaces/srv/odometry_start.hpp>
#include <modelec_interfaces/srv/odometry_get_pid.hpp>
#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;
long 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);
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, 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

8
src/modelec/src/multiple_serial_listener.cpp → src/modelec_com/src/multiple_serial_listener.cpp
View File

@@ -1,4 +1,4 @@
#include "modelec/multiple_serial_listener.hpp"
#include "modelec_com/multiple_serial_listener.hpp"
#include <boost/system/error_code.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/asio.hpp>
@@ -94,7 +94,7 @@ namespace Modelec
MultipleSerialListener::MultipleSerialListener()
: Node("multiple_serial_listener"), io()
{
add_serial_listener_service_ = create_service<modelec_interface::srv::AddSerialListener>(
add_serial_listener_service_ = create_service<modelec_interfaces::srv::AddSerialListener>(
"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<modelec_interface::srv::AddSerialListener::Request> request,
std::shared_ptr<modelec_interface::srv::AddSerialListener::Response> response)
const std::shared_ptr<modelec_interfaces::srv::AddSerialListener::Request> request,
std::shared_ptr<modelec_interfaces::srv::AddSerialListener::Response> response)
{
if (serial_listeners.find(request->name) != serial_listeners.end())
{

12
src/modelec/src/pca9685_controller.cpp → src/modelec_com/src/pca9685_controller.cpp
View File

@@ -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<modelec_interface::msg::PCA9685Servo>(
"servo_control", 10, [this](const modelec_interface::msg::PCA9685Servo::SharedPtr msg) {
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()) {
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<modelec_interface::srv::AddServoMotor>(
"add_servo", [this](const modelec_interface::srv::AddServoMotor::Request::SharedPtr request,
modelec_interface::srv::AddServoMotor::Response::SharedPtr response) {
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()) {
active_servos.insert(request->pin);
response->success = true;

72
src/modelec/src/pcb_alim_interface.cpp → src/modelec_com/src/pcb_alim_interface.cpp
View File

@@ -1,17 +1,17 @@
#include <modelec/pcb_alim_interface.hpp>
#include <modelec/utils.hpp>
#include <modelec_interface/srv/add_serial_listener.hpp>
#include <modelec_com/pcb_alim_interface.hpp>
#include <modelec_utils/utils.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
namespace Modelec
{
PCBAlimInterface::PCBAlimInterface() : Node("pcb_alim_interface")
{
// Service to create a new serial listener
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = "pcb_alim";
request->bauds = 115200;
request->serial_port = "/dev/pts/12"; // TODO : check the real serial port
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("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<modelec_interface::srv::AlimOut>(
pcb_out_service_ = create_service<modelec_interfaces::srv::AlimOut>(
"alim/out",
[this](const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interface::srv::AlimOut::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> 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<modelec_interface::srv::AlimIn>(
pcb_in_service_ = create_service<modelec_interfaces::srv::AlimIn>(
"alim/in",
[this](const std::shared_ptr<modelec_interface::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interface::srv::AlimIn::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimIn::Response> response)
{
HandleGetPCBInData(request, response);
});
pcb_bau_service_ = create_service<modelec_interface::srv::AlimBau>(
pcb_bau_service_ = create_service<modelec_interfaces::srv::AlimBau>(
"alim/bau",
[this](const std::shared_ptr<modelec_interface::srv::AlimBau::Request> request,
std::shared_ptr<modelec_interface::srv::AlimBau::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::AlimBau::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimBau::Response> response)
{
HandleGetPCBBauData(request, response);
});
pcb_emg_service_ = create_service<modelec_interface::srv::AlimEmg>(
pcb_emg_service_ = create_service<modelec_interfaces::srv::AlimEmg>(
"alim/emg",
[this](const std::shared_ptr<modelec_interface::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interface::srv::AlimEmg::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimEmg::Response> response)
{
HandleSetPCBEmgData(request, response);
});
pcb_temp_service_ = create_service<modelec_interface::srv::AlimTemp>(
pcb_temp_service_ = create_service<modelec_interfaces::srv::AlimTemp>(
"alim/temp",
[this](const std::shared_ptr<modelec_interface::srv::AlimTemp::Request> request,
std::shared_ptr<modelec_interface::srv::AlimTemp::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::AlimTemp::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimTemp::Response> response)
{
HandleGetPCBTempData(request, response);
});
pcb_emg_subscriber_ = this->create_subscription<modelec_interface::msg::AlimEmg>(
pcb_emg_subscriber_ = this->create_subscription<modelec_interfaces::msg::AlimEmg>(
"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<modelec_interface::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interface::srv::AlimOut::Response> response)
void PCBAlimInterface::HandleGetPCBOutData(const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response)
{
std::promise<PCBData> promise;
auto future = promise.get_future();
@@ -231,8 +231,8 @@ namespace Modelec
response->result = result.value;
}
void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr<modelec_interface::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interface::srv::AlimOut::Response> response)
void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response)
{
std::promise<PCBData> promise;
auto future = promise.get_future();
@@ -250,8 +250,8 @@ namespace Modelec
response->result = result.value;
}
void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr<modelec_interface::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interface::srv::AlimIn::Response> response)
void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr<modelec_interfaces::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimIn::Response> response)
{
std::promise<PCBData> promise;
auto future = promise.get_future();
@@ -269,8 +269,8 @@ namespace Modelec
response->result = result.value;
}
void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interface::srv::AlimBau::Request>,
std::shared_ptr<modelec_interface::srv::AlimBau::Response> response)
void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interfaces::srv::AlimBau::Request>,
std::shared_ptr<modelec_interfaces::srv::AlimBau::Response> response)
{
std::promise<PCBBau> promise;
auto future = promise.get_future();
@@ -287,8 +287,8 @@ namespace Modelec
response->activate = result.activate;
}
void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr<modelec_interface::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interface::srv::AlimEmg::Response> response)
void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr<modelec_interfaces::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimEmg::Response> response)
{
std::promise<bool> promise;
auto future = promise.get_future();
@@ -304,8 +304,8 @@ namespace Modelec
}
void PCBAlimInterface::HandleGetPCBTempData(
const std::shared_ptr<modelec_interface::srv::AlimTemp::Request>,
std::shared_ptr<modelec_interface::srv::AlimTemp::Response> response)
const std::shared_ptr<modelec_interfaces::srv::AlimTemp::Request>,
std::shared_ptr<modelec_interfaces::srv::AlimTemp::Response> response)
{
std::promise<PCBData> promise;
auto future = promise.get_future();

126
src/modelec/src/pcb_odo_interface.cpp → src/modelec_com/src/pcb_odo_interface.cpp
View File

@@ -1,17 +1,17 @@
#include <modelec/pcb_odo_interface.hpp>
#include <modelec/utils.hpp>
#include <modelec_interface/srv/add_serial_listener.hpp>
#include <modelec_com/pcb_odo_interface.hpp>
#include <modelec_utils/utils.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
namespace Modelec
{
PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface")
{
// Service to create a new serial listener
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = "pcb_odo";
request->bauds = 115200;
request->serial_port = "/dev/pts/7"; // TODO : check the real serial port
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("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<modelec_interface::msg::OdometryPos>(
odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"odometry/get_position", 10);
odo_speed_publisher_ = this->create_publisher<modelec_interface::msg::OdometrySpeed>(
odo_speed_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometrySpeed>(
"odometry/speed", 10);
odo_tof_publisher_ = this->create_publisher<modelec_interface::msg::OdometryToF>(
odo_tof_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryToF>(
"odometry/tof", 10);
odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interface::msg::OdometryWaypointReach>(
odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryWaypointReach>(
"odometry/waypoint_reach", 10);
odo_pid_publisher_ = this->create_publisher<modelec_interface::msg::OdometryPid>(
odo_pid_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPid>(
"odometry/get_pid", 10);
odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interface::msg::OdometryAddWaypoint>(
odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
"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<modelec_interface::msg::OdometryPos>(
odo_set_pos_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
"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<modelec_interface::msg::OdometryPid>(
odo_set_pid_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryPid>(
"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<modelec_interface::srv::OdometryToF>(
get_tof_service_ = create_service<modelec_interfaces::srv::OdometryToF>(
"odometry/tof",
[this](const std::shared_ptr<modelec_interface::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
{
HandleGetTof(request, response);
});
get_speed_service_ = create_service<modelec_interface::srv::OdometrySpeed>(
get_speed_service_ = create_service<modelec_interfaces::srv::OdometrySpeed>(
"odometry/speed",
[this](const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request> request,
std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
{
HandleGetSpeed(request, response);
});
get_position_service_ = create_service<modelec_interface::srv::OdometryPosition>(
get_position_service_ = create_service<modelec_interfaces::srv::OdometryPosition>(
"odometry/get_position",
[this](const std::shared_ptr<modelec_interface::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryPosition::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
{
HandleGetPosition(request, response);
});
set_start_service_ = create_service<modelec_interface::srv::OdometryStart>(
set_start_service_ = create_service<modelec_interfaces::srv::OdometryStart>(
"odometry/start",
[this](const std::shared_ptr<modelec_interface::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryStart::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
{
HandleGetStart(request, response);
});
get_pid_service_ = create_service<modelec_interface::srv::OdometryGetPid>(
get_pid_service_ = create_service<modelec_interfaces::srv::OdometryGetPid>(
"odometry/get_pid",
[this](const std::shared_ptr<modelec_interface::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryGetPid::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
{
HandleGetPID(request, response);
});
set_pid_service_ = create_service<modelec_interface::srv::OdometrySetPid>(
set_pid_service_ = create_service<modelec_interfaces::srv::OdometrySetPid>(
"odometry/set_pid",
[this](const std::shared_ptr<modelec_interface::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interface::srv::OdometrySetPid::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
{
HandleSetPID(request, response);
});
add_waypoint_service_ = create_service<modelec_interface::srv::OdometryAddWaypoint>(
add_waypoint_service_ = create_service<modelec_interfaces::srv::OdometryAddWaypoint>(
"odometry/add_waypoint",
[this](const std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Response> response)
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> 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<modelec_interface::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryToF::Response> response)
const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
{
std::promise<long> promise;
auto future = promise.get_future();
@@ -328,8 +328,8 @@ namespace Modelec
}
void PCBOdoInterface::HandleGetSpeed(
const std::shared_ptr<modelec_interface::srv::OdometrySpeed::Request>,
std::shared_ptr<modelec_interface::srv::OdometrySpeed::Response> response)
const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
{
std::promise<OdometryData> promise;
auto future = promise.get_future();
@@ -348,8 +348,8 @@ namespace Modelec
}
void PCBOdoInterface::HandleGetPosition(
const std::shared_ptr<modelec_interface::srv::OdometryPosition::Request>,
std::shared_ptr<modelec_interface::srv::OdometryPosition::Response> response)
const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
{
std::promise<OdometryData> promise;
auto future = promise.get_future();
@@ -367,8 +367,8 @@ namespace Modelec
response->theta = result.theta;
}
void PCBOdoInterface::HandleGetStart(const std::shared_ptr<modelec_interface::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryStart::Response> response)
void PCBOdoInterface::HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
{
std::promise<bool> promise;
auto future = promise.get_future();
@@ -382,8 +382,8 @@ namespace Modelec
response->success = future.get();
}
void PCBOdoInterface::HandleGetPID(const std::shared_ptr<modelec_interface::srv::OdometryGetPid::Request>,
std::shared_ptr<modelec_interface::srv::OdometryGetPid::Response> response)
void PCBOdoInterface::HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
{
std::promise<PIDData> promise;
auto future = promise.get_future();
@@ -402,8 +402,8 @@ namespace Modelec
response->d = result.d;
}
void PCBOdoInterface::HandleSetPID(const std::shared_ptr<modelec_interface::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interface::srv::OdometrySetPid::Response> response)
void PCBOdoInterface::HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
{
std::promise<bool> promise;
auto future = promise.get_future();
@@ -420,8 +420,8 @@ namespace Modelec
}
void PCBOdoInterface::HandleAddWaypoint(
const std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interface::srv::OdometryAddWaypoint::Response> response)
const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response)
{
std::promise<bool> 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);
}

86
src/modelec/CMakeLists.txt → src/modelec_core/CMakeLists.txt
View File

@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# 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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# 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
$<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_interface)
target_link_libraries(pca9685_controller ${WIRINGPI_LIB})
target_include_directories(pca9685_controller PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
@@ -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

0
src/modelec/LICENSE → src/modelec_core/LICENSE
View File

0
src/modelec/config/gz_bridge.yaml → src/modelec_core/config/gz_bridge.yaml
View File

0
src/modelec/config/rviz_view.rviz → src/modelec_core/config/rviz_view.rviz
View File

0
src/modelec/description/gazebo_control.xacro → src/modelec_core/description/gazebo_control.xacro
View File

0
src/modelec/description/inertial_macros.xacro → src/modelec_core/description/inertial_macros.xacro
View File

0
src/modelec/description/robot.urdf.xacro → src/modelec_core/description/robot.urdf.xacro
View File

0
src/modelec/description/robot_core.xacro → src/modelec_core/description/robot_core.xacro
View File

14
src/modelec/include/modelec/arm_controller.hpp → src/modelec_core/include/modelec/arm_controller.hpp
View File

@@ -2,9 +2,9 @@
#include <array>
#include <rclcpp/rclcpp.hpp>
#include <modelec_interface/msg/pca9685_servo.hpp>
#include <modelec_interface/msg/servo_mode.hpp>
#include <modelec_interface/srv/add_servo_motor.hpp>
#include <modelec_interfaces/msg/pca9685_servo.hpp>
#include <modelec_interfaces/msg/servo_mode.hpp>
#include <modelec_interfaces/srv/add_servo_motor.hpp>
#include <rclcpp/subscription.hpp>
#include <map>
@@ -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<modelec_interface::msg::PCA9685Servo>::SharedPtr servo_spublisher_;
rclcpp::Subscription<modelec_interface::msg::ServoMode>::SharedPtr subscription_;
rclcpp::Publisher<modelec_interfaces::msg::PCA9685Servo>::SharedPtr servo_spublisher_;
rclcpp::Subscription<modelec_interfaces::msg::ServoMode>::SharedPtr subscription_;
std::unordered_map<int, Pince> 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<modelec_interface::srv::AddServoMotor>::SharedPtr client_;
rclcpp::Client<modelec_interfaces::srv::AddServoMotor>::SharedPtr client_;
void ControlPince(const Mode::SharedPtr msg);
void ControlArm(const Mode::SharedPtr msg);

35
src/modelec_core/include/modelec/button_gpio_controller.hpp Normal file
View File

@@ -0,0 +1,35 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/srv/add_button.hpp>
#include <modelec_interfaces/srv/button.hpp>
#include <modelec_interfaces/msg/button.hpp>
#include <wiringPi.h>
namespace Modelec {
class ButtonGpioController : public rclcpp::Node {
struct Button {
int pin;
rclcpp::Publisher<modelec_interfaces::msg::Button>::SharedPtr publisher;
std::string name;
};
public:
ButtonGpioController();
private:
// service
rclcpp::Service<modelec_interfaces::srv::AddButton>::SharedPtr new_button_service_;
rclcpp::Service<modelec_interfaces::srv::Button>::SharedPtr button_server_;
// service callbacks
void new_button(const std::shared_ptr<modelec_interfaces::srv::AddButton::Request> request, std::shared_ptr<modelec_interfaces::srv::AddButton::Response> response);
void check_button(const std::shared_ptr<modelec_interfaces::srv::Button::Request> request, std::shared_ptr<modelec_interfaces::srv::Button::Response> response);
// timer
rclcpp::TimerBase::SharedPtr timer_;
std::map<int, Button> buttons_;
};
}

15
src/modelec/include/modelec/gamecontroller_listener.hpp → src/modelec_core/include/modelec/gamecontroller_listener.hpp
View File

@@ -1,21 +1,20 @@
#pragma once
#include "modelec_interface/msg/pca9685_servo.hpp"
#include "modelec_interfaces/msg/pca9685_servo.hpp"
#include <array>
#include <rclcpp/publisher.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <modelec_interface/msg/servo_mode.hpp>
#include <modelec_interface/msg/pca9685_servo.hpp>
#include <modelec_interface/srv/add_servo_motor.hpp>
#include <modelec/utils.hpp>
#include <modelec_interfaces/msg/servo_mode.hpp>
#include <modelec_interfaces/msg/pca9685_servo.hpp>
#include <modelec_interfaces/srv/add_servo_motor.hpp>
namespace Modelec {
class ControllerListener : public rclcpp::Node
{
using ServoMode = modelec_interface::msg::ServoMode;
using ServoMode = modelec_interfaces::msg::ServoMode;
std::array<int, 3> pinces = {
ServoMode::PINCE_CLOSED,
@@ -45,8 +44,8 @@ namespace Modelec {
rclcpp::Publisher<ServoMode>::SharedPtr servo_publisher_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr odometry_publisher_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr clear_pca_publisher_;
rclcpp::Publisher<modelec_interface::msg::PCA9685Servo>::SharedPtr pca9685_publisher_;
rclcpp::Client<modelec_interface::srv::AddServoMotor>::SharedPtr client_;
rclcpp::Publisher<modelec_interfaces::msg::PCA9685Servo>::SharedPtr pca9685_publisher_;
rclcpp::Client<modelec_interfaces::srv::AddServoMotor>::SharedPtr client_;
void CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg);
void CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg);

0
src/modelec/include/modelec/move_controller.hpp → src/modelec_core/include/modelec/move_controller.hpp
View File

24
src/modelec_core/include/modelec/solenoid_controller.hpp Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <unordered_set>
#include <wiringPi.h>
#include <modelec_interfaces/msg/solenoid.hpp>
#include <modelec_interfaces/srv/add_solenoid.hpp>
namespace Modelec {
class SolenoidController : public rclcpp::Node {
public:
SolenoidController();
private:
std::unordered_set<int> solenoid_pin_;
rclcpp::Subscription<modelec_interfaces::msg::Solenoid>::SharedPtr solenoid_subscriber_;
rclcpp::Service<modelec_interfaces::srv::AddSolenoid>::SharedPtr add_solenoid_service_;
void activateSolenoid(const modelec_interfaces::msg::Solenoid::SharedPtr msg);
void addSolenoid(const std::shared_ptr<modelec_interfaces::srv::AddSolenoid::Request> request, std::shared_ptr<modelec_interfaces::srv::AddSolenoid::Response> response);
};
}

6
src/modelec/include/modelec/speed_result.hpp → src/modelec_core/include/modelec/speed_result.hpp
View File

@@ -1,7 +1,7 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <modelec_interface/msg/odometry_speed.hpp>
#include <modelec_interfaces/msg/odometry_speed.hpp>
#include <fstream>
namespace Modelec
@@ -19,8 +19,8 @@ namespace Modelec
std::ofstream file_;
rclcpp::Subscription<modelec_interface::msg::OdometrySpeed>::SharedPtr sub_speed_;
rclcpp::Subscription<modelec_interfaces::msg::OdometrySpeed>::SharedPtr sub_speed_;
void SpeedCallback(const modelec_interface::msg::OdometrySpeed::SharedPtr msg);
void SpeedCallback(const modelec_interfaces::msg::OdometrySpeed::SharedPtr msg);
};
}

0
src/modelec/launch/launch_sim.launch.py → src/modelec_core/launch/launch_sim.launch.py
View File

8
src/modelec/launch/modelec.launch.yml → src/modelec_core/launch/modelec.launch.yml
View File

@@ -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'

8
src/modelec/launch/modelec_gamecontroller.launch.yml → src/modelec_core/launch/modelec_gamecontroller.launch.yml
View File

@@ -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'

0
src/modelec/launch/modelec_gamecontroller_remote_controller.launch.yml → src/modelec_core/launch/modelec_gamecontroller_remote_controller.launch.yml
View File

8
src/modelec/launch/modelec_gamecontroller_remote_server.launch.yml → src/modelec_core/launch/modelec_gamecontroller_remote_server.launch.yml
View File

@@ -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'

0
src/modelec/launch/rsp.launch.py → src/modelec_core/launch/rsp.launch.py
View File

6
src/modelec/launch/test.modelec.launch.yml → src/modelec_core/launch/test.modelec.launch.yml
View File

@@ -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'

8
src/modelec/package.xml → src/modelec_core/package.xml
View File

@@ -1,18 +1,20 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>modelec</name>
<version>0.0.0</version>
<name>modelec_core</name>
<version>0.1.0</version>
<description>TODO: Package description</description>
<maintainer email="contact@modelec.club">modelec</maintainer>
<license>GPL-3.0-only</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>modelec_interface</depend>
<depend>modelec_interfaces</depend>
<depend>modelec_utils</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geometry_msgs</depend>
<build_depend>boost</build_depend>
<exec_depend>boost</exec_depend>

20
src/modelec/src/arm_controller.cpp → src/modelec_core/src/arm_controller.cpp
View File

@@ -1,10 +1,10 @@
#include "modelec_interface/msg/servo_mode.hpp"
#include "modelec_interfaces/msg/servo_mode.hpp"
#include <modelec/arm_controller.hpp>
namespace Modelec {
ArmController::ArmController() : Node("pince_controller") {
this->servo_spublisher_ = this->create_publisher<modelec_interface::msg::PCA9685Servo>("servo_control", 10);
client_ = this->create_client<modelec_interface::srv::AddServoMotor>("add_servo");
this->servo_spublisher_ = this->create_publisher<modelec_interfaces::msg::PCA9685Servo>("servo_control", 10);
client_ = this->create_client<modelec_interfaces::srv::AddServoMotor>("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<modelec_interface::srv::AddServoMotor::Request>();
auto request = std::make_shared<modelec_interfaces::srv::AddServoMotor::Request>();
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<modelec_interface::msg::ServoMode>(
"arm_control", 10, [this](const modelec_interface::msg::ServoMode::SharedPtr msg) {
subscription_ = this->create_subscription<modelec_interfaces::msg::ServoMode>(
"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);

114
src/modelec/src/button_gpio_controller.cpp → src/modelec_core/src/button_gpio_controller.cpp
View File

@@ -1,57 +1,57 @@
#include <modelec/button_gpio_controller.hpp>
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<modelec_interface::srv::AddButton>("add_button", std::bind(&ButtonGpioController::new_button, this, std::placeholders::_1, std::placeholders::_2));
button_server_ = create_service<modelec_interface::srv::Button>("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<modelec_interface::srv::AddButton::Request> request, std::shared_ptr<modelec_interface::srv::AddButton::Response> 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<modelec_interface::msg::Button>("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<modelec_interface::srv::Button::Request> request, std::shared_ptr<modelec_interface::srv::Button::Response> 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<Modelec::ButtonGpioController>());
rclcpp::shutdown();
return 0;
}
#include <modelec/button_gpio_controller.hpp>
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<modelec_interfaces::srv::AddButton>("add_button", std::bind(&ButtonGpioController::new_button, this, std::placeholders::_1, std::placeholders::_2));
button_server_ = create_service<modelec_interfaces::srv::Button>("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<modelec_interfaces::srv::AddButton::Request> request, std::shared_ptr<modelec_interfaces::srv::AddButton::Response> 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<modelec_interfaces::msg::Button>("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<modelec_interfaces::srv::Button::Request> request, std::shared_ptr<modelec_interfaces::srv::Button::Response> 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<Modelec::ButtonGpioController>());
rclcpp::shutdown();
return 0;
}

18
src/modelec/src/gamecontroller_listener.cpp → src/modelec_core/src/gamecontroller_listener.cpp
View File

@@ -1,6 +1,6 @@
#include "modelec/gamecontroller_listener.hpp"
#include "modelec/utils.hpp"
#include <modelec_interface/srv/add_serial_listener.hpp>
#include "modelec_utils/utils.hpp"
#include <modelec_interfaces/srv/add_serial_listener.hpp>
namespace Modelec
{
@@ -14,11 +14,11 @@ namespace Modelec
servo_publisher_ = this->create_publisher<ServoMode>("arm_control", 10);
// Service to create a new serial listener
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = "odometry";
request->bauds = 115200;
request->serial_port = "/dev/ttyACM0";
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("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<std_msgs::msg::Empty>("clear_pca9685", 10);
pca9685_publisher_ = this->create_publisher<modelec_interface::msg::PCA9685Servo>(
pca9685_publisher_ = this->create_publisher<modelec_interfaces::msg::PCA9685Servo>(
"servo_control", 10);
}
else
@@ -60,7 +60,7 @@ namespace Modelec
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
client_ = this->create_client<modelec_interface::srv::AddServoMotor>("add_servo");
client_ = this->create_client<modelec_interfaces::srv::AddServoMotor>("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<modelec_interface::srv::AddServoMotor::Request>();
auto request = std::make_shared<modelec_interfaces::srv::AddServoMotor::Request>();
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<int>(Modelec::mapValue(static_cast<float>(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<int>(Modelec::mapValue(static_cast<float>(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);

0
src/modelec/src/move_controller.cpp → src/modelec_core/src/move_controller.cpp
View File

10
src/modelec/src/solenoid_controller.cpp → src/modelec_core/src/solenoid_controller.cpp
View File

@@ -5,22 +5,22 @@ namespace Modelec {
SolenoidController::SolenoidController() : Node("solenoid_controller") {
wiringPiSetup();
solenoid_subscriber_ = this->create_subscription<modelec_interface::msg::Solenoid>(
solenoid_subscriber_ = this->create_subscription<modelec_interfaces::msg::Solenoid>(
"solenoid", 10, std::bind(&SolenoidController::activateSolenoid, this, std::placeholders::_1));
add_solenoid_service_ = this->create_service<modelec_interface::srv::AddSolenoid>(
add_solenoid_service_ = this->create_service<modelec_interfaces::srv::AddSolenoid>(
"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<modelec_interface::srv::AddSolenoid::Request> request,
std::shared_ptr<modelec_interface::srv::AddSolenoid::Response> response) {
void SolenoidController::addSolenoid(const std::shared_ptr<modelec_interfaces::srv::AddSolenoid::Request> request,
std::shared_ptr<modelec_interfaces::srv::AddSolenoid::Response> response) {
if (solenoid_pin_.find(request->pin) != solenoid_pin_.end()) {
response->success = false;
return;

4
src/modelec/src/speed_result.cpp → src/modelec_core/src/speed_result.cpp
View File

@@ -7,7 +7,7 @@ namespace Modelec {
std::cerr << "Error opening file: " << fileName_ << std::endl;
}
sub_speed_ = this->create_subscription<modelec_interface::msg::OdometrySpeed>(
sub_speed_ = this->create_subscription<modelec_interfaces::msg::OdometrySpeed>(
"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 << ","

0
src/modelec/worlds/empty.world → src/modelec_core/worlds/empty.world
View File

0
src/modelec/worlds/gazebo.world → src/modelec_core/worlds/gazebo.world
View File

0
src/modelec/worlds/obstacles.world → src/modelec_core/worlds/obstacles.world
View File

4
src/modelec_gui/CMakeLists.txt
View File

@@ -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

26
src/modelec_gui/include/modelec_gui/pages/test_page.hpp
View File

@@ -7,12 +7,12 @@
#include <rclcpp/rclcpp.hpp>
#include <modelec_interface/msg/odometry_pos.hpp>
#include <modelec_interface/msg/odometry_add_waypoint.hpp>
#include <modelec_interface/srv/odometry_speed.hpp>
#include <modelec_interface/srv/odometry_start.hpp>
#include <modelec_interface/srv/odometry_get_pid.hpp>
#include <modelec_interface/srv/odometry_set_pid.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/srv/odometry_speed.hpp>
#include <modelec_interfaces/srv/odometry_start.hpp>
#include <modelec_interfaces/srv/odometry_get_pid.hpp>
#include <modelec_interfaces/srv/odometry_set_pid.hpp>
namespace ModelecGUI {
@@ -45,17 +45,17 @@ namespace ModelecGUI {
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<modelec_interface::msg::OdometryPos>::SharedPtr sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr sub_;
rclcpp::Publisher<modelec_interface::msg::OdometryAddWaypoint>::SharedPtr pub_add_waypoint_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr pub_add_waypoint_;
// client
rclcpp::Client<modelec_interface::srv::OdometrySpeed>::SharedPtr client_;
rclcpp::Client<modelec_interface::srv::OdometryStart>::SharedPtr client_start_;
rclcpp::Client<modelec_interface::srv::OdometryGetPid>::SharedPtr client_get_pid_;
rclcpp::Client<modelec_interface::srv::OdometrySetPid>::SharedPtr client_set_pid_;
rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedPtr client_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedPtr client_get_pid_;
rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::SharedPtr client_set_pid_;
void PositionCallback(const modelec_interface::msg::OdometryPos::SharedPtr msg);
void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
};
} // namespace Modelec

2
src/modelec_gui/package.xml
View File

@@ -9,7 +9,7 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>modelec_interface</depend>
<depend>modelec_interfaces</depend>
<depend>rclcpp</depend>
<depend>std_msgs</depend>

34
src/modelec_gui/src/pages/test_page.cpp
View File

@@ -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<modelec_interface::srv::OdometryStart::Request>();
auto request = std::make_shared<modelec_interfaces::srv::OdometryStart::Request>();
request->start = true;
client_start_->async_send_request(request, [this](rclcpp::Client<modelec_interface::srv::OdometryStart>::SharedFuture response) {
client_start_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryStart>::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<modelec_interface::srv::OdometryGetPid::Request>();
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_interface::srv::OdometryGetPid>::SharedFuture response) {
client_get_pid_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::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<modelec_interface::srv::OdometrySetPid::Request>();
auto request = std::make_shared<modelec_interfaces::srv::OdometrySetPid::Request>();
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<modelec_interface::srv::OdometrySetPid>::SharedFuture response) {
client_set_pid_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::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<modelec_interface::srv::OdometrySpeed::Request>();
auto request = std::make_shared<modelec_interfaces::srv::OdometrySpeed::Request>();
// Make the asynchronous service call
client_->async_send_request(request, [this](rclcpp::Client<modelec_interface::srv::OdometrySpeed>::SharedFuture response) {
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));
@@ -120,7 +120,7 @@ namespace ModelecGUI
startTest_ = new QPushButton("Start Test");
connect(startTest_, &QPushButton::clicked, this, [this]() {
auto firstRequest = std::make_shared<modelec_interface::msg::OdometryAddWaypoint>();
auto firstRequest = std::make_shared<modelec_interfaces::msg::OdometryAddWaypoint>();
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<modelec_interface::msg::OdometryAddWaypoint>();
auto secondRequest = std::make_shared<modelec_interfaces::msg::OdometryAddWaypoint>();
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<modelec_interface::msg::OdometryPos>(
sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"/odometry/get_position", 10,
std::bind(&TestPage::PositionCallback, this, std::placeholders::_1));
pub_add_waypoint_ = node_->create_publisher<modelec_interface::msg::OdometryAddWaypoint>(
pub_add_waypoint_ = node_->create_publisher<modelec_interfaces::msg::OdometryAddWaypoint>(
"/odometry/add_waypoint", 10);
// Set up service client
client_ = node_->create_client<modelec_interface::srv::OdometrySpeed>("odometry/speed");
client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("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<modelec_interface::srv::OdometryStart>("odometry/start");
client_start_ = node_->create_client<modelec_interfaces::srv::OdometryStart>("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<modelec_interface::srv::OdometryGetPid>("odometry/get_pid");
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()) {
@@ -190,7 +190,7 @@ namespace ModelecGUI
RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again...");
}
client_set_pid_ = node_->create_client<modelec_interface::srv::OdometrySetPid>("odometry/set_pid");
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()) {
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));

2
src/modelec_interface/CMakeLists.txt → src/modelec_interfaces/CMakeLists.txt
View File

@@ -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)

0
src/modelec_interface/LICENSE → src/modelec_interfaces/LICENSE
View File

0
src/modelec_interface/msg/Alim/AlimEmg.msg → src/modelec_interfaces/msg/Alim/AlimEmg.msg
View File

0
src/modelec_interface/msg/Button.msg → src/modelec_interfaces/msg/Button.msg
View File

0
src/modelec_interface/msg/Odometry/OdometryAddWaypoint.msg → src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg
View File

0
src/modelec_interface/msg/Odometry/OdometryPos.msg → src/modelec_interfaces/msg/Odometry/OdometryPos.msg
View File

0
src/modelec_interface/msg/Odometry/OdometrySpeed.msg → src/modelec_interfaces/msg/Odometry/OdometrySpeed.msg
View File

0
src/modelec_interface/msg/Odometry/OdometryStart.msg → src/modelec_interfaces/msg/Odometry/OdometryStart.msg
View File

0
src/modelec_interface/msg/Odometry/OdometryToF.msg → src/modelec_interfaces/msg/Odometry/OdometryToF.msg
View File

0
src/modelec_interface/msg/Odometry/OdometryWaypointReach.msg → src/modelec_interfaces/msg/Odometry/OdometryWaypointReach.msg
View File

0
src/modelec_interface/msg/Odometry/PID/OdometryPid.msg → src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg
View File

0
src/modelec_interface/msg/PCA9685Servo.msg → src/modelec_interfaces/msg/PCA9685Servo.msg
View File

0
src/modelec_interface/msg/ServoMode.msg → src/modelec_interfaces/msg/ServoMode.msg
View File

0
src/modelec_interface/msg/Solenoid.msg → src/modelec_interfaces/msg/Solenoid.msg
View File

2
src/modelec_interface/package.xml → src/modelec_interfaces/package.xml
View File

@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>modelec_interface</name>
<name>modelec_interfaces</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="contact@modelec.club">modelec</maintainer>

0
src/modelec_interface/srv/AddButton.srv → src/modelec_interfaces/srv/AddButton.srv
View File

0
src/modelec_interface/srv/AddSerialListener.srv → src/modelec_interfaces/srv/AddSerialListener.srv
View File

0
src/modelec_interface/srv/AddServoMotor.srv → src/modelec_interfaces/srv/AddServoMotor.srv
View File

0
src/modelec_interface/srv/AddSolenoid.srv → src/modelec_interfaces/srv/AddSolenoid.srv
View File

0
src/modelec_interface/srv/Alim/AlimBau.srv → src/modelec_interfaces/srv/Alim/AlimBau.srv
View File

0
src/modelec_interface/srv/Alim/AlimEmg.srv → src/modelec_interfaces/srv/Alim/AlimEmg.srv
View File

0
src/modelec_interface/srv/Alim/AlimIn.srv → src/modelec_interfaces/srv/Alim/AlimIn.srv
View File

0
src/modelec_interface/srv/Alim/AlimOut.srv → src/modelec_interfaces/srv/Alim/AlimOut.srv
View File

0
src/modelec_interface/srv/Alim/AlimTemp.srv → src/modelec_interfaces/srv/Alim/AlimTemp.srv
View File

0
src/modelec_interface/srv/Button.srv → src/modelec_interfaces/srv/Button.srv
View File

0
src/modelec_interface/srv/Odometry/OdometryAddWaypoint.srv → src/modelec_interfaces/srv/Odometry/OdometryAddWaypoint.srv
View File

0
src/modelec_interface/srv/Odometry/OdometryPosition.srv → src/modelec_interfaces/srv/Odometry/OdometryPosition.srv
View File

0
src/modelec_interface/srv/Odometry/OdometrySpeed.srv → src/modelec_interfaces/srv/Odometry/OdometrySpeed.srv
View File

0
src/modelec_interface/srv/Odometry/OdometryStart.srv → src/modelec_interfaces/srv/Odometry/OdometryStart.srv
View File

0
src/modelec_interface/srv/Odometry/OdometryToF.srv → src/modelec_interfaces/srv/Odometry/OdometryToF.srv
View File

0
src/modelec_interface/srv/Odometry/PID/OdometryGetPid.srv → src/modelec_interfaces/srv/Odometry/PID/OdometryGetPid.srv
View File

0
src/modelec_interface/srv/Odometry/PID/OdometrySetPid.srv → src/modelec_interfaces/srv/Odometry/PID/OdometrySetPid.srv
View File

0
src/modelec_interface/srv/Tirette.srv → src/modelec_interfaces/srv/Tirette.srv
View File

0
src/modelec/include/modelec/lidar_controller.hpp → src/modelec_sensors/include/modelec_sensors/lidar_controller.hpp
View File

4
src/modelec/include/modelec/tirette_controller.hpp → src/modelec_sensors/include/modelec_sensors/tirette_controller.hpp
View File

@@ -1,14 +1,14 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <modelec_interface/srv/tirette.hpp>
#include <modelec_interfaces/srv/tirette.hpp>
#include <std_msgs/msg/bool.hpp>
#include <wiringPi.h>
#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 {

2
src/modelec/src/lidar_controller.cpp → src/modelec_sensors/src/lidar_controller.cpp
View File

@@ -1,4 +1,4 @@
#include <modelec/lidar_controller.hpp>
#include <modelec_sensors/lidar_controller.hpp>
namespace Modelec {
LidarController::LidarController() : Node("lidar_controller") {

4
src/modelec/src/tirette_controller.cpp → src/modelec_sensors/src/tirette_controller.cpp
View File

@@ -1,4 +1,4 @@
#include <modelec/tirette_controller.hpp>
#include <modelec_sensors/tirette_controller.hpp>
#include <wiringPi.h>
namespace Modelec {
@@ -17,7 +17,7 @@ namespace Modelec {
tirette_state = false;
// Initialize the service
service = this->create_service<modelec_interface::srv::Tirette>(
service = this->create_service<modelec_interfaces::srv::Tirette>(
"tirette",
std::bind(&TiretteController::check_tirette, this, std::placeholders::_1, std::placeholders::_2));

24
src/modelec_utils/include/modelec_utils/utils.hpp Normal file
View File

@@ -0,0 +1,24 @@
#pragma once
#include <string>
#include <vector>
#include <sstream>
namespace Modelec {
#define PI 3.14159265358979323846
std::vector<std::string> split(const std::string &s, char delim);
std::string join(const std::vector<std::string> &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 <typename T, typename std::enable_if<std::is_arithmetic<T>::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));
}
}

41
src/modelec_utils/src/utils.cpp Normal file
View File

@@ -0,0 +1,41 @@
#include <modelec_utils/utils.hpp>
namespace Modelec {
std::vector<std::string> split(const std::string &s, char delim) {
std::vector<std::string> 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<std::string>& 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;
}
};