mirror of
https://github.com/modelec/modelec-marcel-ROS.git
synced 2026-01-18 16:47:43 +01:00
refactor with the doc
This commit is contained in:
@@ -31,9 +31,9 @@ target_include_directories(serial_listener PUBLIC
|
||||
)
|
||||
|
||||
# USB Arduino Logic Processor Node
|
||||
add_executable(usb_arduino_logic_processor src/usb_arduino_logic_processor.cpp)
|
||||
ament_target_dependencies(usb_arduino_logic_processor rclcpp std_msgs modelec_interface)
|
||||
target_include_directories(usb_arduino_logic_processor PUBLIC
|
||||
add_executable(odometry_logic_processor src/odometry_logic_processor.cpp)
|
||||
ament_target_dependencies(odometry_logic_processor rclcpp std_msgs modelec_interface)
|
||||
target_include_directories(odometry_logic_processor PUBLIC
|
||||
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
|
||||
$<INSTALL_INTERFACE:include>
|
||||
)
|
||||
@@ -119,7 +119,7 @@ endif()
|
||||
# Install targets
|
||||
install(TARGETS
|
||||
serial_listener
|
||||
usb_arduino_logic_processor
|
||||
odometry_logic_processor
|
||||
pca9685_controller
|
||||
gamecontroller_listener
|
||||
move_controller
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <modelec_interface/msg/pca9685_servo.hpp>
|
||||
#include <modelec_interface/msg/servo_mode.hpp>
|
||||
#include <modelec_interface/srv/new_servo_motor.hpp>
|
||||
#include <modelec_interface/srv/add_servo_motor.hpp>
|
||||
#include <rclcpp/subscription.hpp>
|
||||
#include <map>
|
||||
|
||||
@@ -52,7 +52,7 @@ namespace Modelec {
|
||||
};
|
||||
|
||||
// service lient to add a servo
|
||||
rclcpp::Client<modelec_interface::srv::NewServoMotor>::SharedPtr client_;
|
||||
rclcpp::Client<modelec_interface::srv::AddServoMotor>::SharedPtr client_;
|
||||
|
||||
void ControlPince(const Mode::SharedPtr msg);
|
||||
void ControlArm(const Mode::SharedPtr msg);
|
||||
|
||||
@@ -1,7 +1,7 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <modelec_interface/srv/new_button.hpp>
|
||||
#include <modelec_interface/srv/add_button.hpp>
|
||||
#include <modelec_interface/srv/button.hpp>
|
||||
#include <modelec_interface/msg/button.hpp>
|
||||
#include <wiringPi.h>
|
||||
@@ -20,11 +20,11 @@ namespace Modelec {
|
||||
|
||||
private:
|
||||
// service
|
||||
rclcpp::Service<modelec_interface::srv::NewButton>::SharedPtr new_button_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::NewButton::Request> request, std::shared_ptr<modelec_interface::srv::NewButton::Response> response);
|
||||
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
|
||||
|
||||
@@ -9,7 +9,7 @@
|
||||
#include <sensor_msgs/msg/joy.hpp>
|
||||
#include <modelec_interface/msg/servo_mode.hpp>
|
||||
#include <modelec_interface/msg/pca9685_servo.hpp>
|
||||
#include <modelec_interface/srv/new_servo_motor.hpp>
|
||||
#include <modelec_interface/srv/add_servo_motor.hpp>
|
||||
#include <modelec/utils.hpp>
|
||||
|
||||
namespace Modelec {
|
||||
@@ -43,10 +43,10 @@ namespace Modelec {
|
||||
|
||||
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr subscription_;
|
||||
rclcpp::Publisher<ServoMode>::SharedPtr servo_publisher_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr arduino_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::NewServoMotor>::SharedPtr client_;
|
||||
rclcpp::Client<modelec_interface::srv::AddServoMotor>::SharedPtr client_;
|
||||
|
||||
void CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg);
|
||||
void CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg);
|
||||
|
||||
@@ -11,17 +11,11 @@ namespace Modelec {
|
||||
MoveController();
|
||||
|
||||
private:
|
||||
rclcpp::TimerBase::SharedPtr timer;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher;
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
|
||||
|
||||
float speedX, speedZ;
|
||||
float x = 0, y = 0, theta = 0;
|
||||
|
||||
float x, y, z, theta;
|
||||
float x_target, y_target, z_target, theta_target;
|
||||
|
||||
void move();
|
||||
|
||||
void move_target_callback(const std_msgs::msg::String::SharedPtr msg);
|
||||
void PublishPosition();
|
||||
};
|
||||
}
|
||||
@@ -4,7 +4,6 @@
|
||||
#include <boost/asio.hpp>
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
#include <modelec_interface/srv/add_serial_listener.hpp>
|
||||
#include <thread>
|
||||
|
||||
#define MAX_MESSAGE_LEN 1048
|
||||
#define READ_REFRESH_RATE 100 //ms
|
||||
|
||||
19
src/modelec/include/modelec/odometry_logic_processor.hpp
Normal file
19
src/modelec/include/modelec/odometry_logic_processor.hpp
Normal file
@@ -0,0 +1,19 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
#include <modelec/utils.hpp>
|
||||
#include <modelec_interface/msg/odometry_data.hpp>
|
||||
|
||||
namespace Modelec {
|
||||
class odometry_logic_processor : public rclcpp::Node {
|
||||
public:
|
||||
odometry_logic_processor();
|
||||
private:
|
||||
void processData(const std::string &data);
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_to_odometry_;
|
||||
rclcpp::Publisher<modelec_interface::msg::OdometryData>::SharedPtr publisher_odometry_;
|
||||
};
|
||||
}
|
||||
@@ -6,7 +6,7 @@
|
||||
#include <wiringPiI2C.h>
|
||||
|
||||
#include <modelec_interface/msg/pca9685_servo.hpp>
|
||||
#include <modelec_interface/srv/new_servo_motor.hpp>
|
||||
#include <modelec_interface/srv/add_servo_motor.hpp>
|
||||
#include <std_msgs/msg/empty.hpp>
|
||||
|
||||
#define PCA9685_FREQUENCY 25000000.0
|
||||
@@ -31,7 +31,7 @@ namespace Modelec {
|
||||
std::unordered_set<int> active_servos;
|
||||
|
||||
// service to add a servo
|
||||
rclcpp::Service<modelec_interface::srv::NewServoMotor>::SharedPtr add_servo_service_;
|
||||
rclcpp::Service<modelec_interface::srv::AddServoMotor>::SharedPtr add_servo_service_;
|
||||
|
||||
OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;
|
||||
rcl_interfaces::msg::SetParametersResult onParameterChange(const std::vector<rclcpp::Parameter> ¶meters);
|
||||
|
||||
@@ -4,7 +4,7 @@
|
||||
#include <unordered_set>
|
||||
#include <wiringPi.h>
|
||||
#include <modelec_interface/msg/solenoid.hpp>
|
||||
#include <modelec_interface/srv/new_solenoid.hpp>
|
||||
#include <modelec_interface/srv/add_solenoid.hpp>
|
||||
|
||||
namespace Modelec {
|
||||
class SolenoidController : public rclcpp::Node {
|
||||
@@ -15,10 +15,10 @@ namespace Modelec {
|
||||
|
||||
rclcpp::Subscription<modelec_interface::msg::Solenoid>::SharedPtr solenoid_subscriber_;
|
||||
|
||||
rclcpp::Service<modelec_interface::srv::NewSolenoid>::SharedPtr add_solenoid_service_;
|
||||
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::NewSolenoid::Request> request, std::shared_ptr<modelec_interface::srv::NewSolenoid::Response> response);
|
||||
void addSolenoid(const std::shared_ptr<modelec_interface::srv::AddSolenoid::Request> request, std::shared_ptr<modelec_interface::srv::AddSolenoid::Response> response);
|
||||
};
|
||||
}
|
||||
|
||||
@@ -1,18 +0,0 @@
|
||||
#pragma once
|
||||
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include <std_msgs/msg/string.hpp>
|
||||
#include <modelec/utils.hpp>
|
||||
#include <modelec_interface/msg/arduino_data.hpp>
|
||||
|
||||
namespace Modelec {
|
||||
class LogicProcessor : public rclcpp::Node {
|
||||
public:
|
||||
LogicProcessor();
|
||||
private:
|
||||
void processData(const std::string &data);
|
||||
|
||||
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
|
||||
rclcpp::Publisher<modelec_interface::msg::ArduinoData>::SharedPtr publisher_;
|
||||
};
|
||||
}
|
||||
@@ -4,7 +4,7 @@
|
||||
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::NewServoMotor>("add_servo");
|
||||
client_ = this->create_client<modelec_interface::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::NewServoMotor::Request>();
|
||||
auto request = std::make_shared<modelec_interface::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) {
|
||||
|
||||
@@ -8,7 +8,7 @@ namespace Modelec {
|
||||
rclcpp::shutdown();
|
||||
}
|
||||
|
||||
new_button_service_ = create_service<modelec_interface::srv::NewButton>("new_button", std::bind(&ButtonGpioController::new_button, this, std::placeholders::_1, std::placeholders::_2));
|
||||
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_) {
|
||||
@@ -20,7 +20,7 @@ namespace Modelec {
|
||||
});
|
||||
}
|
||||
|
||||
void ButtonGpioController::new_button(const std::shared_ptr<modelec_interface::srv::NewButton::Request> request, std::shared_ptr<modelec_interface::srv::NewButton::Response> response) {
|
||||
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;
|
||||
|
||||
@@ -14,7 +14,7 @@ namespace Modelec {
|
||||
|
||||
// Service to create a new serial listener
|
||||
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
|
||||
request->name = "arduino";
|
||||
request->name = "odometry";
|
||||
request->bauds = 115200;
|
||||
request->serial_port = "/dev/ttyACM0";
|
||||
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
|
||||
@@ -27,7 +27,7 @@ namespace Modelec {
|
||||
}
|
||||
auto result = client->async_send_request(request);
|
||||
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) {
|
||||
if (!result.get()->status) {
|
||||
if (!result.get()->success) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "Added serial listener");
|
||||
@@ -36,13 +36,13 @@ namespace Modelec {
|
||||
RCLCPP_ERROR(this->get_logger(), "Service call failed");
|
||||
}
|
||||
|
||||
arduino_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
|
||||
odometry_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
|
||||
|
||||
clear_pca_publisher_ = this->create_publisher<std_msgs::msg::Empty>("clear_pca9685", 10);
|
||||
|
||||
pca9685_publisher_ = this->create_publisher<modelec_interface::msg::PCA9685Servo>("servo_control", 10);
|
||||
|
||||
client_ = this->create_client<modelec_interface::srv::NewServoMotor>("add_servo");
|
||||
client_ = this->create_client<modelec_interface::srv::AddServoMotor>("add_servo");
|
||||
|
||||
// ensure the server is up
|
||||
while (!client_->wait_for_service(std::chrono::seconds(1))) {
|
||||
@@ -54,7 +54,7 @@ namespace Modelec {
|
||||
}
|
||||
|
||||
for (auto servo : solarPannelServos) {
|
||||
auto request = std::make_shared<modelec_interface::srv::NewServoMotor::Request>();
|
||||
auto request = std::make_shared<modelec_interface::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) == rclcpp::FutureReturnCode::SUCCESS) {
|
||||
@@ -156,7 +156,7 @@ namespace Modelec {
|
||||
}
|
||||
auto message = std_msgs::msg::String();
|
||||
message.data = "W";
|
||||
arduino_publisher_->publish(message);
|
||||
odometry_publisher_->publish(message);
|
||||
button_4_was_pressed = true;
|
||||
} else {
|
||||
button_4_was_pressed = false;
|
||||
@@ -185,7 +185,7 @@ namespace Modelec {
|
||||
|
||||
if (speed != last_speed) {
|
||||
message.data = "V " + std::to_string(speed);
|
||||
arduino_publisher_->publish(message);
|
||||
odometry_publisher_->publish(message);
|
||||
last_speed = speed;
|
||||
}
|
||||
|
||||
@@ -198,7 +198,7 @@ namespace Modelec {
|
||||
|
||||
if (rotation != last_rotation) {
|
||||
message.data = "R " + std::to_string(rotation);
|
||||
arduino_publisher_->publish(message);
|
||||
odometry_publisher_->publish(message);
|
||||
last_rotation = rotation;
|
||||
}
|
||||
|
||||
|
||||
@@ -3,61 +3,20 @@
|
||||
namespace Modelec {
|
||||
MoveController::MoveController() : Node("move_controller")
|
||||
{
|
||||
// Initialize the speed
|
||||
speedX = 0.0;
|
||||
speedZ = 0.0;
|
||||
|
||||
// Initialize the target position
|
||||
x_target = 0.0;
|
||||
y_target = 0.0;
|
||||
z_target = 0.0;
|
||||
theta_target = 0.0;
|
||||
|
||||
// Initialize the publisher
|
||||
publisher = this->create_publisher<std_msgs::msg::String>("robot_position", 10);
|
||||
publisher_ = this->create_publisher<std_msgs::msg::String>("robot_position", 10);
|
||||
|
||||
// Initialize the subscriber
|
||||
subscriber = this->create_subscription<std_msgs::msg::String>(
|
||||
"move_target", 10,
|
||||
std::bind(&MoveController::move_target_callback, this, std::placeholders::_1));
|
||||
|
||||
// Initialize the timer
|
||||
timer = this->create_wall_timer(std::chrono::milliseconds(REFRESH_RATE), std::bind(&MoveController::move, this));
|
||||
timer_ = this->create_wall_timer(
|
||||
std::chrono::milliseconds(REFRESH_RATE),
|
||||
std::bind(&MoveController::PublishPosition, this)
|
||||
);
|
||||
}
|
||||
|
||||
void MoveController::move()
|
||||
void MoveController::PublishPosition()
|
||||
{
|
||||
// Move the robot
|
||||
x += speedX;
|
||||
z += speedZ;
|
||||
|
||||
// Check if the robot has reached the target position
|
||||
if ((x >= x_target && x+speedX < x_target) || (x <= x_target && x+speedX > x_target))
|
||||
{
|
||||
speedX = 0.0;
|
||||
}
|
||||
|
||||
if ((z >= z_target && z+speedZ < z_target) || (z <= z_target && z+speedZ > z_target))
|
||||
{
|
||||
speedZ = 0.0;
|
||||
}
|
||||
|
||||
// Prepare and publish the message
|
||||
auto msg = std_msgs::msg::String();
|
||||
msg.data = std::to_string(x) + " " + std::to_string(y) + " " + std::to_string(z) + " " + std::to_string(theta);
|
||||
publisher->publish(msg);
|
||||
}
|
||||
|
||||
void MoveController::move_target_callback(const std_msgs::msg::String::SharedPtr msg)
|
||||
{
|
||||
// Parse the target position
|
||||
std::istringstream iss(msg->data);
|
||||
iss >> x_target >> y_target >> z_target >> theta_target;
|
||||
|
||||
// Calculate the speed
|
||||
speedX = (x_target - x) / 100;
|
||||
speedZ = (z_target - z) / 100;
|
||||
RCLCPP_INFO(this->get_logger(), "Target position: %f %f %f %f", x_target, y_target, z_target, theta_target);
|
||||
std_msgs::msg::String msg;
|
||||
msg.data = std::to_string(x) + ";" + std::to_string(y) + ";" + std::to_string(theta);
|
||||
publisher_->publish(msg);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@@ -24,7 +24,7 @@ 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) {
|
||||
if (serial_listeners.find(request->name) != serial_listeners.end()) {
|
||||
response->status = true;
|
||||
response->success = true;
|
||||
response->publisher = serial_listeners[request->name]->publisher_->get_topic_name();
|
||||
response->subscriber = serial_listeners[request->name]->subscriber_->get_topic_name();
|
||||
return;
|
||||
@@ -33,7 +33,7 @@ namespace Modelec {
|
||||
auto listener = std::make_shared<SerialListener>(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN, io);
|
||||
|
||||
if (!listener->IsOk()) {
|
||||
response->status = false;
|
||||
response->success = false;
|
||||
return;
|
||||
}
|
||||
|
||||
@@ -45,7 +45,7 @@ namespace Modelec {
|
||||
|
||||
serial_listeners.insert({request->name, listener});
|
||||
|
||||
response->status = true;
|
||||
response->success = true;
|
||||
response->publisher = listener->publisher_->get_topic_name();
|
||||
response->subscriber = listener->subscriber_->get_topic_name();
|
||||
}
|
||||
|
||||
@@ -1,15 +1,13 @@
|
||||
#include "modelec/usb_aruido_logic_processor.hpp"
|
||||
#include "modelec/odometry_logic_processor.hpp"
|
||||
#include "modelec_interface/srv/add_serial_listener.hpp"
|
||||
#include <vector>
|
||||
#include <modelec_interface/srv/add_serial_listener.hpp>
|
||||
|
||||
using namespace Modelec;
|
||||
|
||||
LogicProcessor::LogicProcessor() : Node("usb_logic_processor") {
|
||||
Modelec::odometry_logic_processor::odometry_logic_processor() : Node("usb_logic_processor") {
|
||||
publisher_odometry_ = this->create_publisher<modelec_interface::msg::OdometryData>("odomertry_data", 10);
|
||||
|
||||
// Service to create a new serial listener
|
||||
auto request = std::make_shared<modelec_interface::srv::AddSerialListener::Request>();
|
||||
request->name = "arduino";
|
||||
request->name = "odometry";
|
||||
request->bauds = 115200;
|
||||
request->serial_port = "/dev/pts/6";
|
||||
auto client = this->create_client<modelec_interface::srv::AddSerialListener>("add_serial_listener");
|
||||
@@ -23,7 +21,7 @@ LogicProcessor::LogicProcessor() : Node("usb_logic_processor") {
|
||||
auto result = client->async_send_request(request);
|
||||
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) {
|
||||
if (auto res = result.get()) {
|
||||
if (res->status) {
|
||||
if (res->success) {
|
||||
|
||||
RCLCPP_INFO(this->get_logger(), "Publisher: %s", res->publisher.c_str());
|
||||
RCLCPP_INFO(this->get_logger(), "Subscriber: %s", res->subscriber.c_str());
|
||||
@@ -33,7 +31,7 @@ LogicProcessor::LogicProcessor() : Node("usb_logic_processor") {
|
||||
processData(msg->data);
|
||||
});
|
||||
|
||||
publisher_ = this->create_publisher<modelec_interface::msg::ArduinoData>(res->subscriber, 10);
|
||||
publisher_to_odometry_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
|
||||
} else {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
|
||||
}
|
||||
@@ -45,16 +43,16 @@ LogicProcessor::LogicProcessor() : Node("usb_logic_processor") {
|
||||
}
|
||||
}
|
||||
|
||||
void LogicProcessor::processData(const std::string &data) {
|
||||
void Modelec::odometry_logic_processor::processData(const std::string &data) {
|
||||
auto processed_data = "Processed: " + data;
|
||||
RCLCPP_INFO(this->get_logger(), "%s", processed_data.c_str());
|
||||
auto msg = modelec_interface::msg::ArduinoData();
|
||||
auto msg = modelec_interface::msg::OdometryData();
|
||||
std::vector<std::string> d = split(data, ':');
|
||||
|
||||
if (d.size() == 3) {
|
||||
msg.x = std::stof(d[0]);
|
||||
msg.y = std::stof(d[1]);
|
||||
msg.theta = std::stof(d[2]);
|
||||
msg.x = std::stol(d[0]);
|
||||
msg.y = std::stol(d[1]);
|
||||
msg.theta = std::stol(d[2]);
|
||||
}
|
||||
else {
|
||||
msg.x = 0;
|
||||
@@ -62,13 +60,13 @@ void LogicProcessor::processData(const std::string &data) {
|
||||
msg.theta = 0;
|
||||
}
|
||||
|
||||
publisher_->publish(msg);
|
||||
publisher_odometry_->publish(msg);
|
||||
}
|
||||
|
||||
int main(int argc, char ** argv)
|
||||
{
|
||||
rclcpp::init(argc, argv);
|
||||
rclcpp::spin(std::make_shared<Modelec::LogicProcessor>());
|
||||
rclcpp::spin(std::make_shared<Modelec::odometry_logic_processor>());
|
||||
rclcpp::shutdown();
|
||||
return 0;
|
||||
}
|
||||
@@ -32,9 +32,9 @@ namespace Modelec {
|
||||
this->clearAllDevices();
|
||||
});
|
||||
|
||||
add_servo_service_ = this->create_service<modelec_interface::srv::NewServoMotor>(
|
||||
"add_servo", [this](const modelec_interface::srv::NewServoMotor::Request::SharedPtr request,
|
||||
modelec_interface::srv::NewServoMotor::Response::SharedPtr response) {
|
||||
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) {
|
||||
if (active_servos.find(request->pin) == active_servos.end()) {
|
||||
active_servos.insert(request->pin);
|
||||
response->success = true;
|
||||
|
||||
@@ -20,7 +20,7 @@ namespace Modelec
|
||||
}
|
||||
auto result = client->async_send_request(request);
|
||||
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) == rclcpp::FutureReturnCode::SUCCESS) {
|
||||
if (!result.get()->status) {
|
||||
if (!result.get()->success) {
|
||||
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
|
||||
} else {
|
||||
RCLCPP_INFO(this->get_logger(), "Added serial listener");
|
||||
|
||||
@@ -8,7 +8,7 @@ namespace Modelec {
|
||||
solenoid_subscriber_ = this->create_subscription<modelec_interface::msg::Solenoid>(
|
||||
"solenoid", 10, std::bind(&SolenoidController::activateSolenoid, this, std::placeholders::_1));
|
||||
|
||||
add_solenoid_service_ = this->create_service<modelec_interface::srv::NewSolenoid>(
|
||||
add_solenoid_service_ = this->create_service<modelec_interface::srv::AddSolenoid>(
|
||||
"add_solenoid", std::bind(&SolenoidController::addSolenoid, this, std::placeholders::_1, std::placeholders::_2));
|
||||
}
|
||||
|
||||
@@ -19,8 +19,8 @@ namespace Modelec {
|
||||
}
|
||||
}
|
||||
|
||||
void SolenoidController::addSolenoid(const std::shared_ptr<modelec_interface::srv::NewSolenoid::Request> request,
|
||||
std::shared_ptr<modelec_interface::srv::NewSolenoid::Response> response) {
|
||||
void SolenoidController::addSolenoid(const std::shared_ptr<modelec_interface::srv::AddSolenoid::Request> request,
|
||||
std::shared_ptr<modelec_interface::srv::AddSolenoid::Response> response) {
|
||||
if (solenoid_pin_.find(request->pin) != solenoid_pin_.end()) {
|
||||
response->success = false;
|
||||
return;
|
||||
|
||||
@@ -26,6 +26,7 @@ namespace Modelec {
|
||||
|
||||
// Initialize the timer
|
||||
timer = this->create_wall_timer(std::chrono::milliseconds(REFRESH_RATE), [this]() {
|
||||
// TODO : change that to publish in continue (so change the main program)
|
||||
bool lastState = tirette_state;
|
||||
tirette_state = digitalRead(GPIO_TIRETTE) == LOW;
|
||||
|
||||
|
||||
@@ -11,16 +11,15 @@ find_package(std_msgs REQUIRED)
|
||||
find_package(rosidl_default_generators REQUIRED)
|
||||
|
||||
rosidl_generate_interfaces(${PROJECT_NAME}
|
||||
"msg/ArduinoData.msg"
|
||||
"msg/OdometryData.msg"
|
||||
"msg/PCA9685Servo.msg"
|
||||
"msg/ServoMoteur.msg"
|
||||
"msg/ServoMode.msg"
|
||||
"msg/Solenoid.msg"
|
||||
"msg/Button.msg"
|
||||
"srv/NewServoMotor.srv"
|
||||
"srv/NewSolenoid.srv"
|
||||
"srv/AddServoMotor.srv"
|
||||
"srv/AddSolenoid.srv"
|
||||
"srv/Tirette.srv"
|
||||
"srv/NewButton.srv"
|
||||
"srv/AddButton.srv"
|
||||
"srv/Button.srv"
|
||||
"srv/AddSerialListener.srv"
|
||||
DEPENDENCIES std_msgs
|
||||
|
||||
@@ -1,2 +1,2 @@
|
||||
int64 pin
|
||||
uint8 pin
|
||||
bool state
|
||||
@@ -1,2 +0,0 @@
|
||||
int64 pin
|
||||
bool state
|
||||
@@ -1,2 +1,2 @@
|
||||
int64 pin
|
||||
uint8 pin
|
||||
bool state
|
||||
@@ -1,4 +1,4 @@
|
||||
int64 pin
|
||||
uint8 pin
|
||||
string name
|
||||
---
|
||||
bool success
|
||||
@@ -2,6 +2,6 @@ string name
|
||||
string serial_port
|
||||
int64 bauds
|
||||
---
|
||||
bool status
|
||||
bool success
|
||||
string publisher
|
||||
string subscriber
|
||||
@@ -1,3 +1,3 @@
|
||||
int64 pin
|
||||
uint8 pin
|
||||
---
|
||||
bool success
|
||||
@@ -1,3 +1,3 @@
|
||||
int64 pin
|
||||
uint8 pin
|
||||
---
|
||||
bool success
|
||||
@@ -1,4 +1,4 @@
|
||||
int64 pin
|
||||
uint8 pin
|
||||
string name
|
||||
---
|
||||
bool status
|
||||
Reference in New Issue
Block a user