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

update new listener

Browse Source
This commit is contained in:
acki
2025-12-11 18:51:32 +01:00
parent dcc064a0f9
commit 4e277ef0ca
15 changed files with 61 additions and 2221 deletions
Download Patch File Download Diff File Expand all files Collapse all files

59
src/modelec_com/CMakeLists.txt
View File

@@ -9,85 +9,34 @@ endif()
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(ament_index_cpp REQUIRED)
find_package(modelec_interfaces REQUIRED)
find_package(modelec_utils REQUIRED)
# Serial listener Node
add_executable(serial_listener src/multiple_serial_listener.cpp)
ament_target_dependencies(serial_listener rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(serial_listener Boost::system modelec_utils::utils modelec_utils::config)
target_include_directories(serial_listener PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# PCB Odometry Node
add_executable(pcb_odo_interface src/pcb_odo_interface.cpp)
add_executable(pcb_odo_interface src/pcb_odo_interface.cpp src/serial_listener.cpp)
ament_target_dependencies(pcb_odo_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(pcb_odo_interface modelec_utils::utils modelec_utils::config)
target_link_libraries(pcb_odo_interface Boost::system modelec_utils::utils modelec_utils::config)
target_include_directories(pcb_odo_interface PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# PCB Odometry Node
add_executable(pcb_odo_interface_new src/pcb_odo_interface.new.cpp src/serial_listener.cpp)
ament_target_dependencies(pcb_odo_interface_new rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(pcb_odo_interface_new modelec_utils::utils modelec_utils::config)
target_include_directories(pcb_odo_interface_new PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# PCB Action Node
add_executable(pcb_action_interface_new src/pcb_action_interface.new.cpp src/serial_listener.cpp)
ament_target_dependencies(pcb_action_interface_new rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(pcb_action_interface_new modelec_utils::utils modelec_utils::config)
target_include_directories(pcb_action_interface_new PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# PCB Alim Node
add_executable(pcb_alim_interface src/pcb_alim_interface.cpp)
ament_target_dependencies(pcb_alim_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(pcb_alim_interface modelec_utils::utils modelec_utils::config)
target_include_directories(pcb_alim_interface PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# PCB Action Node
add_executable(pcb_action_interface src/pcb_action_interface.cpp)
add_executable(pcb_action_interface src/pcb_action_interface.cpp src/serial_listener.cpp)
ament_target_dependencies(pcb_action_interface rclcpp std_msgs modelec_interfaces ament_index_cpp)
target_link_libraries(pcb_action_interface modelec_utils::utils modelec_utils::config)
target_link_libraries(pcb_action_interface Boost::system modelec_utils::utils modelec_utils::config)
target_include_directories(pcb_action_interface PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
# Enable testing and linting
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
# Install targets
install(TARGETS
serial_listener
pcb_alim_interface
pcb_odo_interface
pcb_odo_interface_new
pcb_action_interface
pcb_action_interface_new
DESTINATION lib/${PROJECT_NAME}
)

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

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

26
src/modelec_com/include/modelec_com/pcb_action_interface.hpp
View File

@@ -1,5 +1,7 @@
#pragma once
#include <modelec_com/serial_listener.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
@@ -11,14 +13,11 @@
namespace Modelec
{
class PCBActionInterface : public rclcpp::Node
class PCBActionInterface : public rclcpp::Node, public SerialListener
{
public:
PCBActionInterface();
rclcpp::CallbackGroup::SharedPtr pcb_callback_group_;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> pcb_executor_;
std::thread pcb_executor_thread_;
~PCBActionInterface() override;
@@ -31,10 +30,7 @@ namespace Modelec
std::map<int, bool> relay_value_;
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);
void read(const std::string& msg) override;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_sub_;
@@ -67,16 +63,14 @@ namespace Modelec
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_disarm_sub_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tir_arm_set_sub_;
bool isOk = false;
public:
void SendToPCB(const std::string& data) const;
void SendToPCB(const std::string& data);
void SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data = {}) const;
const std::vector<std::string>& data = {});
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 SendMove(const std::string& elem, const std::vector<std::string>& data = {}) const;
void RespondEvent(const std::string& elem, const std::vector<std::string>& data = {}) const;
void GetData(const std::string& elem, const std::vector<std::string>& data = {});
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
void SendMove(const std::string& elem, const std::vector<std::string>& data = {});
void RespondEvent(const std::string& elem, const std::vector<std::string>& data = {});
};
}

76
src/modelec_com/include/modelec_com/pcb_action_interface.new.hpp
View File

@@ -1,76 +0,0 @@
#pragma once
#include <modelec_com/serial_listener.hpp>
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <std_msgs/msg/bool.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_relay_state.hpp>
#include <modelec_interfaces/msg/action_servo_pos.hpp>
namespace Modelec
{
class PCBActionInterface : public rclcpp::Node, public SerialListener
{
public:
PCBActionInterface();
~PCBActionInterface() override;
protected:
std::map<int, int> asc_value_mapper_;
std::map<int, std::map<int, double>> servo_pos_mapper_;
int asc_state_ = 0;
std::map<int, int> servo_value_;
std::map<int, bool> relay_value_;
private:
void read(const std::string& msg) override;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_get_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_get_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_get_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_set_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_set_res_pub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_sub_;
rclcpp::Subscription<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_sub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionAscPos>::SharedPtr asc_move_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionServoPos>::SharedPtr servo_move_res_pub_;
rclcpp::Publisher<modelec_interfaces::msg::ActionRelayState>::SharedPtr relay_move_res_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_start_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_arm_pub_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr tir_disarm_pub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_start_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_disarm_sub_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tir_arm_set_sub_;
public:
void SendToPCB(const std::string& data);
void SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data = {});
void GetData(const std::string& elem, const std::vector<std::string>& data = {});
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
void SendMove(const std::string& elem, const std::vector<std::string>& data = {});
void RespondEvent(const std::string& elem, const std::vector<std::string>& data = {});
};
}

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

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

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

@@ -1,5 +1,7 @@
#pragma once
#include <modelec_com/serial_listener.hpp>
#include <rclcpp/rclcpp.hpp>
#include <queue>
@@ -30,14 +32,11 @@
namespace Modelec
{
class PCBOdoInterface : public rclcpp::Node
class PCBOdoInterface : public rclcpp::Node, public SerialListener
{
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
@@ -55,10 +54,7 @@ namespace Modelec
};
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);
void read(const std::string& msg) override;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_pos_publisher_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr odo_get_pos_sub_;
@@ -83,8 +79,6 @@ namespace Modelec
int timeout_ms = 1000;
int attempt = 5;
bool isOk = false;
public:
void SendToPCB(const std::string& data);
void SendToPCB(const std::string& order, const std::string& elem,

108
src/modelec_com/include/modelec_com/pcb_odo_interface.new.hpp
View File

@@ -1,108 +0,0 @@
#pragma once
#include <modelec_com/serial_listener.hpp>
#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.hpp>
#include <modelec_interfaces/msg/odometry_waypoints.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>
#include <std_msgs/msg/bool.hpp>
#include <std_msgs/msg/empty.hpp>
namespace Modelec
{
class PCBOdoInterface : public rclcpp::Node, public SerialListener
{
public:
PCBOdoInterface();
~PCBOdoInterface() override;
struct OdometryData
{
long x;
long y;
double theta;
};
struct PIDData
{
float p;
float i;
float d;
};
private:
void read(const std::string& msg) override;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_pos_publisher_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr odo_get_pos_sub_;
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::OdometryWaypoint>::SharedPtr odo_waypoint_reach_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_pid_publisher_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr odo_add_waypoints_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::OdometryWaypoint::SharedPtr msg);
void AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg);
void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr start_odo_sub_;
bool start_odo_ = false;
int timeout_ms = 1000;
int attempt = 5;
public:
void SendToPCB(const std::string& data);
void SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data = {});
void GetData(const std::string& elem, const std::vector<std::string>& data = {});
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
void GetPos();
void GetSpeed();
void GetToF(const int& tof);
void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
void SetRobotPos(long x, long y, double theta);
void AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg);
void AddWaypoint(modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);
void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta);
void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg);
void SetStart(bool start);
void GetPID();
void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
void SetPID(std::string name, float p, float i, float d, std::optional<float> min = std::nullopt, std::optional<float> max = std::nullopt);
};
} // namespace Modelec

2
src/modelec_com/package.xml
View File

@@ -13,8 +13,6 @@
<depend>modelec_core</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>

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

@@ -1,223 +0,0 @@
#include "modelec_com/multiple_serial_listener.hpp"
#include <boost/system/error_code.hpp>
#include <boost/asio/serial_port.hpp>
#include <boost/asio.hpp>
#include <chrono>
#include <thread>
#include <modelec_utils/utils.hpp>
namespace Modelec
{
SerialListener::SerialListener(const std::string& name, int bauds, const std::string& serial_port,
int max_message_len, boost::asio::io_service& io)
: bauds_(bauds), serial_port_(serial_port), max_message_len_(max_message_len), io_(io), name_(name), port_(io)
{
try
{
port_.open(serial_port_);
port_.set_option(boost::asio::serial_port_base::baud_rate(bauds_));
status_ = true;
}
catch (boost::system::system_error& e)
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Failed to open serial port: %s", e.what());
status_ = false;
return;
}
read_buffer_.resize(max_message_len_);
start_async_read();
io_thread_ = std::thread([this]()
{
try
{
io_.run();
}
catch (const std::exception& e)
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "IO thread exception: %s", e.what());
}
});
}
SerialListener::~SerialListener()
{
if (status_)
{
close();
}
}
void SerialListener::close()
{
if (status_)
{
if (port_.is_open()) port_.close();
io_.stop();
if (io_thread_.joinable()) io_thread_.join();
status_ = false;
}
}
void SerialListener::start_async_read()
{
if (!status_) start_async_read();
port_.async_read_some(
boost::asio::buffer(read_buffer_),
[this](const boost::system::error_code& ec, std::size_t bytes_transferred)
{
if (!ec && bytes_transferred > 0)
{
std::string d = std::string(read_buffer_.begin(), read_buffer_.begin() + bytes_transferred);
auto allMess = Modelec::split(d, '\n');
for (const auto& mess : allMess)
{
if (!mess.empty())
{
// RCLCPP_INFO(rclcpp::get_logger("SerialListener"), "Received message: %s", mess.c_str());
auto msg = std_msgs::msg::String();
msg.data = mess;
if (publisher_)
{
publisher_->publish(msg);
}
}
}
start_async_read(); // continue reading
}
else
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async read error: %s", ec.message().c_str());
}
});
}
void SerialListener::write(std_msgs::msg::String::SharedPtr msg)
{
std::lock_guard<std::mutex> lock(write_mutex_);
bool write_in_progress = !write_queue_.empty();
write_queue_.push_back(msg->data);
if (!write_in_progress)
{
start_async_write();
}
}
void SerialListener::start_async_write()
{
if (write_queue_.empty()) return;
boost::asio::async_write(
port_,
boost::asio::buffer(write_queue_.front()),
[this](const boost::system::error_code& ec, std::size_t /*length*/)
{
std::lock_guard<std::mutex> lock(write_mutex_);
if (!ec)
{
write_queue_.pop_front();
if (!write_queue_.empty())
{
start_async_write(); // continue writing
}
}
else
{
RCLCPP_ERROR(rclcpp::get_logger("SerialListener"), "Async write error: %s", ec.message().c_str());
}
});
}
MultipleSerialListener::MultipleSerialListener()
: Node("multiple_serial_listener"), io()
{
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));
}
MultipleSerialListener::~MultipleSerialListener()
{
for (auto& listener : serial_listeners)
{
listener.second->close();
}
}
void MultipleSerialListener::add_serial_listener(
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())
{
response->success = true;
response->publisher = serial_listeners[request->name]->publisher_->get_topic_name();
response->subscriber = serial_listeners[request->name]->subscriber_->get_topic_name();
return;
}
RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Adding serial listener: %s", request->name.c_str());
auto listener = std::make_shared<SerialListener>(request->name, request->bauds, request->serial_port,
MAX_MESSAGE_LEN, io);
if (!listener->IsOk())
{
response->success = false;
return;
}
listener->publisher_ = create_publisher<std_msgs::msg::String>("raw_data/" + request->name, 10);
listener->subscriber_ = create_subscription<std_msgs::msg::String>(
"send_to_serial/" + request->name, rclcpp::QoS(10).reliability(rclcpp::ReliabilityPolicy::BestEffort),
std::bind(&SerialListener::write, listener.get(), std::placeholders::_1));
serial_listeners.insert({request->name, listener});
response->success = true;
response->publisher = listener->publisher_->get_topic_name();
response->subscriber = listener->subscriber_->get_topic_name();
RCLCPP_INFO(rclcpp::get_logger("MultipleSerialListener"), "Serial listener %s fully created",
request->name.c_str());
}
rcl_interfaces::msg::SetParametersResult MultipleSerialListener::onParameterChange(
const std::vector<rclcpp::Parameter>& parameters)
{
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
for (const auto& parameter : parameters)
{
if (parameter.get_name() == "bauds" || parameter.get_name() == "serial_port" || parameter.get_name() ==
"max_message_len")
{
updateConnection();
}
}
return result;
}
void MultipleSerialListener::updateConnection()
{
for (auto& listener : serial_listeners)
{
listener.second->SetMaxMessageLen(get_parameter("max_message_len").as_int());
}
}
}
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::MultipleSerialListener>());
rclcpp::shutdown();
return 0;
}

101
src/modelec_com/src/pcb_action_interface.cpp
View File

@@ -1,11 +1,11 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_com/pcb_action_interface.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
#include <modelec_utils/utils.hpp>
namespace Modelec
{
PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface")
PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface"), SerialListener()
{
// Service to create a new serial listener
declare_parameter<std::string>("serial_port", "/dev/USB_ACTION");
@@ -17,62 +17,7 @@ namespace Modelec
request->bauds = get_parameter("baudrate").as_int();
request->serial_port = get_parameter("serial_port").as_string();
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (auto res = result.get())
{
if (res->success)
{
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;
options.callback_group = pcb_callback_group_;
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
res->publisher, 10,
[this](const std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
}, options);
pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
pcb_executor_thread_ = std::thread([this]()
{
pcb_executor_->spin();
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
isOk = true;
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN);
asc_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"action/get/asc", 10,
@@ -265,20 +210,13 @@ namespace Modelec
PCBActionInterface::~PCBActionInterface()
{
if (pcb_executor_)
{
pcb_executor_->cancel();
}
if (pcb_executor_thread_.joinable())
{
pcb_executor_thread_.join();
}
}
void PCBActionInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
void PCBActionInterface::read(const std::string& msg)
{
RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str());
std::vector<std::string> tokens = split(trim(msg->data), ';');
RCLCPP_INFO_ONCE(this->get_logger(), "Received message: '%s'", msg.c_str());
RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received message: '%s'", msg.c_str());
std::vector<std::string> tokens = split(trim(msg), ';');
if (tokens.size() < 2)
{
@@ -393,7 +331,7 @@ namespace Modelec
}
else
{
RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", msg->data.c_str());
RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", msg.c_str());
}
}
else if (tokens[0] == "KO")
@@ -436,7 +374,7 @@ namespace Modelec
}
else
{
RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", msg->data.c_str());
RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", msg.c_str());
}
}
else if (tokens[0] == "EVENT")
@@ -462,18 +400,17 @@ namespace Modelec
}
}
void PCBActionInterface::SendToPCB(const std::string& data) const
void PCBActionInterface::SendToPCB(const std::string& data)
{
if (pcb_publisher_)
if (IsOk())
{
auto message = std_msgs::msg::String();
message.data = data;
pcb_publisher_->publish(message);
RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str());
this->write(data);
}
}
void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data) const
const std::vector<std::string>& data)
{
std::string command = order + ";" + elem;
for (const auto& d : data)
@@ -485,27 +422,28 @@ namespace Modelec
SendToPCB(command);
}
void PCBActionInterface::GetData(const std::string& elem, const std::vector<std::string>& data) const
void PCBActionInterface::GetData(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("GET", elem, data);
}
void PCBActionInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data) const
void PCBActionInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("SET", elem, data);
}
void PCBActionInterface::SendMove(const std::string& elem, const std::vector<std::string>& data) const
void PCBActionInterface::SendMove(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("MOV", elem, data);
}
void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector<std::string>& data) const
void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("ACK", elem, data);
}
}
#ifndef MODELEC_COM_TESTING
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
@@ -520,3 +458,4 @@ int main(int argc, char** argv)
rclcpp::shutdown();
return 0;
}
#endif

461
src/modelec_com/src/pcb_action_interface.new.cpp
View File

@@ -1,461 +0,0 @@
#include <modelec_com/pcb_action_interface.new.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
#include <modelec_utils/utils.hpp>
namespace Modelec
{
PCBActionInterface::PCBActionInterface() : Node("pcb_action_interface"), SerialListener()
{
// Service to create a new serial listener
declare_parameter<std::string>("serial_port", "/dev/USB_ACTION");
declare_parameter<int>("baudrate", 115200);
declare_parameter<std::string>("name", "pcb_action");
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = get_parameter("name").as_string();
request->bauds = get_parameter("baudrate").as_int();
request->serial_port = get_parameter("serial_port").as_string();
this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN);
asc_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"action/get/asc", 10,
[this](const modelec_interfaces::msg::ActionAscPos::SharedPtr)
{
GetData("ASC", {"POS"});
});
servo_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPos>(
"action/get/servo", 10,
[this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
{
GetData("SERVO" + std::to_string(msg->id), {"POS"});
});
relay_get_sub_ = this->create_subscription<modelec_interfaces::msg::ActionRelayState>(
"action/get/relay", 10,
[this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg)
{
GetData("RELAY" + std::to_string(msg->id), {"STATE"});
});
asc_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionAscPos>(
"action/get/asc/res", 10);
servo_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
"action/get/servo/res", 10);
relay_get_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayState>(
"action/get/relay/res", 10);
asc_set_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"action/set/asc", 10,
[this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg)
{
SendOrder("ASC", {std::to_string(msg->pos), std::to_string(msg->value)});
});
servo_set_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPos>(
"action/set/servo", 10,
[this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
{
SendOrder("SERVO" + std::to_string(msg->id), {
"POS" + std::to_string(msg->pos), std::to_string(static_cast<int>(msg->angle * 100))
});
});
asc_set_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionAscPos>(
"action/set/asc/res", 10);
servo_set_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
"action/set/servo/res", 10);
asc_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionAscPos>(
"action/move/asc", 10,
[this](const modelec_interfaces::msg::ActionAscPos::SharedPtr msg)
{
SendMove("ASC", {std::to_string(msg->pos)});
});
servo_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionServoPos>(
"action/move/servo", 10,
[this](const modelec_interfaces::msg::ActionServoPos::SharedPtr msg)
{
SendMove("SERVO" + std::to_string(msg->id), {"POS" + std::to_string(msg->pos)});
});
relay_move_sub_ = this->create_subscription<modelec_interfaces::msg::ActionRelayState>(
"action/move/relay", 10,
[this](const modelec_interfaces::msg::ActionRelayState::SharedPtr msg)
{
SendMove("RELAY" + std::to_string(msg->id), {std::to_string(msg->state)});
});
asc_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionAscPos>(
"action/move/asc/res", 10);
servo_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionServoPos>(
"action/move/servo/res", 10);
relay_move_res_pub_ = this->create_publisher<modelec_interfaces::msg::ActionRelayState>(
"action/move/relay/res", 10);
tir_start_pub_ = this->create_publisher<std_msgs::msg::Empty>(
"action/tir/start", 10);
tir_start_sub_ = this->create_subscription<std_msgs::msg::Empty>(
"action/tir/start/res", 10,
[this](const std_msgs::msg::Empty::SharedPtr)
{
RespondEvent("TIR", {"START"});
});
tir_arm_pub_ = this->create_publisher<std_msgs::msg::Empty>(
"action/tir/arm", 10);
tir_arm_sub_ = this->create_subscription<std_msgs::msg::Empty>(
"action/tir/arm/res", 10,
[this](const std_msgs::msg::Empty::SharedPtr)
{
RespondEvent("TIR", {"ARM"});
});
tir_disarm_pub_ = this->create_publisher<std_msgs::msg::Empty>(
"action/tir/disarm", 10);
tir_disarm_sub_ = this->create_subscription<std_msgs::msg::Empty>(
"action/tir/disarm/res", 10,
[this](const std_msgs::msg::Empty::SharedPtr)
{
RespondEvent("TIR", {"DIS"});
});
tir_arm_set_sub_ = this->create_subscription<std_msgs::msg::Bool>(
"action/tir/arm/set", 10,
[this](const std_msgs::msg::Bool::SharedPtr msg)
{
SendOrder("TIR", {"ARM", msg->data ? "1" : "0"});
});
// TODO : check for real value there
asc_value_mapper_ = {
{0, 0},
{1, 100},
{2, 200},
{3, 300}
};
/*for (auto & [id, v] : asc_value_mapper_)
{
SendOrder("ASC", {std::to_string(id), std::to_string(v)});
}*/
asc_state_ = 3;
// SendMove("ASC", {std::to_string(asc_state_)});
servo_pos_mapper_ = {
{0, {{0, 0.55}, {1, 0}}},
{1, {{0, 0}, {1, 0.4}}},
{2, {{0, M_PI_2}}},
{3, {{0, M_PI_2}}},
{4, {{0, 1.25}, {1, 0.45}}},
{5, {{0, 0}, {1, M_PI}}},
};
for (auto & [id, v] : servo_pos_mapper_)
{
if (id == 5) continue;
for (auto & [key, angle] : v)
{
rclcpp::sleep_for(std::chrono::milliseconds(100));
SendOrder("SERVO" + std::to_string(id), {"POS" + std::to_string(key), std::to_string(static_cast<int>(angle * 100))});
}
}
servo_value_ = {
{0, 1},
{1, 1},
{2, 0},
{3, 0},
{4, 1},
{5, 0}
};
for (auto & [id, v] : servo_value_)
{
rclcpp::sleep_for(std::chrono::milliseconds(100));
SendMove("SERVO" + std::to_string(id), {"POS" + std::to_string(v)});
}
relay_value_ = {
{1, false},
{2, false},
{3, false},
};
for (auto & [id, v] : relay_value_)
{
rclcpp::sleep_for(std::chrono::milliseconds(100));
SendMove("RELAY" + std::to_string(id), {std::to_string(v)});
}
rclcpp::sleep_for(std::chrono::milliseconds(100));
SendOrder("TIR", {"ARM", "1"});
}
PCBActionInterface::~PCBActionInterface()
{
}
void PCBActionInterface::read(const std::string& msg)
{
RCLCPP_INFO_ONCE(this->get_logger(), "Received message: '%s'", msg.c_str());
RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received message: '%s'", msg.c_str());
std::vector<std::string> tokens = split(trim(msg), ';');
if (tokens.size() < 2)
{
RCLCPP_ERROR(this->get_logger(), "Invalid message format");
return;
}
if (tokens[0] == "SET")
{
if (tokens[1] == "ASC")
{
asc_state_ = std::stoi(tokens[3]);
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = asc_state_;
asc_msg.value = asc_value_mapper_[asc_state_];
asc_msg.success = true;
asc_get_res_pub_->publish(asc_msg);
}
else if (startsWith(tokens[1], "SERVO"))
{
int servo_id = std::stoi(tokens[1].substr(5));
int v = std::stoi(tokens[3]);
servo_value_[servo_id] = v;
modelec_interfaces::msg::ActionServoPos servo_msg;
servo_msg.id = servo_id;
servo_msg.pos = v;
servo_msg.angle = servo_pos_mapper_[servo_id][v];
servo_msg.success = true;
servo_get_res_pub_->publish(servo_msg);
}
else if (startsWith(tokens[1], "RELAY"))
{
int relay_id = std::stoi(tokens[1].substr(5));
bool state = (tokens[3] == "1");
relay_value_[relay_id] = state;
modelec_interfaces::msg::ActionRelayState relay_msg;
relay_msg.id = relay_id;
relay_msg.state = state;
relay_msg.success = true;
relay_get_res_pub_->publish(relay_msg);
}
}
else if (tokens[0] == "OK")
{
if (tokens.size() == 4)
{
if (tokens[1] == "ASC")
{
int pos = std::stoi(tokens[2]);
int v = std::stoi(tokens[3]);
asc_value_mapper_[pos] = v;
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = pos;
asc_msg.value = v;
asc_msg.success = true;
asc_set_res_pub_->publish(asc_msg);
}
else if (startsWith(tokens[1], "SERVO"))
{
int servo_id = std::stoi(tokens[1].substr(5));
int key = std::stoi(tokens[2].substr(3));
int v = std::stoi(tokens[3]);
servo_pos_mapper_[servo_id][key] = v;
modelec_interfaces::msg::ActionServoPos servo_msg;
servo_msg.id = servo_id;
servo_msg.pos = key;
servo_msg.angle = v;
servo_msg.success = true;
servo_set_res_pub_->publish(servo_msg);
}
}
else if (tokens.size() == 3)
{
if (tokens[1] == "ASC")
{
asc_state_ = std::stoi(tokens[2]);
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.pos = asc_state_;
asc_msg.success = true;
asc_move_res_pub_->publish(asc_msg);
}
else if (startsWith(tokens[1], "SERVO"))
{
int servo_id = std::stoi(tokens[1].substr(5));
int key = std::stoi(tokens[2].substr(3));
servo_value_[servo_id] = key;
modelec_interfaces::msg::ActionServoPos servo_msg;
servo_msg.id = servo_id;
servo_msg.pos = key;
servo_msg.success = true;
servo_move_res_pub_->publish(servo_msg);
}
else if (startsWith(tokens[1], "RELAY"))
{
int relay_id = std::stoi(tokens[1].substr(5));
bool state = (tokens[2] == "1");
relay_value_[relay_id] = state;
modelec_interfaces::msg::ActionRelayState relay_msg;
relay_msg.id = relay_id;
relay_msg.state = state;
relay_msg.success = true;
relay_move_res_pub_->publish(relay_msg);
}
}
else
{
RCLCPP_WARN(this->get_logger(), "Unknown message format for OK response: '%s'", msg.c_str());
}
}
else if (tokens[0] == "KO")
{
if (tokens.size() == 4)
{
if (tokens[1] == "ASC")
{
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.success = false;
asc_set_res_pub_->publish(asc_msg);
}
else if (startsWith(tokens[1], "SERVO"))
{
modelec_interfaces::msg::ActionServoPos servo_msg;
servo_msg.success = false;
servo_set_res_pub_->publish(servo_msg);
}
}
else if (tokens.size() == 3)
{
if (tokens[1] == "ASC")
{
modelec_interfaces::msg::ActionAscPos asc_msg;
asc_msg.success = false;
asc_move_res_pub_->publish(asc_msg);
}
else if (startsWith(tokens[1], "SERVO"))
{
modelec_interfaces::msg::ActionServoPos servo_msg;
servo_msg.success = false;
servo_move_res_pub_->publish(servo_msg);
}
else if (startsWith(tokens[1], "RELAY"))
{
modelec_interfaces::msg::ActionRelayState relay_msg;
relay_msg.success = false;
relay_move_res_pub_->publish(relay_msg);
}
}
else
{
RCLCPP_WARN(this->get_logger(), "Unknown message format: '%s'", msg.c_str());
}
}
else if (tokens[0] == "EVENT")
{
if (tokens[1] == "TIR")
{
if (tokens[2] == "START")
{
tir_start_pub_->publish(std_msgs::msg::Empty());
RespondEvent(tokens[1], {tokens[2]});
}
else if (tokens[2] == "ARM")
{
tir_arm_pub_->publish(std_msgs::msg::Empty());
RespondEvent(tokens[1], {tokens[2]});
}
else if (tokens[2] == "DIS")
{
tir_disarm_pub_->publish(std_msgs::msg::Empty());
RespondEvent(tokens[1], {tokens[2]});
}
}
}
}
void PCBActionInterface::SendToPCB(const std::string& data)
{
if (IsOk())
{
RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str());
this->write(data);
}
}
void PCBActionInterface::SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data)
{
std::string command = order + ";" + elem;
for (const auto& d : data)
{
command += ";" + d;
}
command += "\n";
SendToPCB(command);
}
void PCBActionInterface::GetData(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("GET", elem, data);
}
void PCBActionInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("SET", elem, data);
}
void PCBActionInterface::SendMove(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("MOV", elem, data);
}
void PCBActionInterface::RespondEvent(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("ACK", elem, data);
}
}
#ifndef MODELEC_COM_TESTING
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Modelec::PCBActionInterface>();
// Increase number of threads explicitly!
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 2 /* or more threads! */);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}
#endif

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

@@ -1,516 +0,0 @@
#include <modelec_com/pcb_alim_interface.hpp>
#include <modelec_utils/utils.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
PCBAlimInterface::PCBAlimInterface() : Node("pcb_alim_interface")
{
// Service to create a new serial listener
declare_parameter<std::string>("serial_port", "/dev/ttyUSB0");
declare_parameter<int>("baudrate", 115200);
declare_parameter<std::string>("name", "pcb_alim");
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = get_parameter("name").as_string();
request->bauds = get_parameter("baudrate").as_int();
request->serial_port = get_parameter("serial_port").as_string();
RCLCPP_INFO(this->get_logger(), "Requesting serial listener with parameters: "
"name='%s', bauds=%ld, serial_port='%s'",
request->name.c_str(), request->bauds, request->serial_port.c_str());
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (auto res = result.get())
{
if (res->success)
{
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;
options.callback_group = pcb_callback_group_;
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
res->publisher, 10,
[this](const std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
}, options);
pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
pcb_executor_thread_ = std::thread([this]()
{
pcb_executor_->spin();
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
isOk = true;
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
pcb_out_service_ = create_service<modelec_interfaces::srv::AlimOut>(
"alim/out",
[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_interfaces::srv::AlimOut::Request::STATE)
{
HandleSetPCBOutData(request, response);
}
else
{
HandleGetPCBOutData(request, response);
}
});
pcb_in_service_ = create_service<modelec_interfaces::srv::AlimIn>(
"alim/in",
[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_interfaces::srv::AlimBau>(
"alim/bau",
[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_interfaces::srv::AlimEmg>(
"alim/emg",
[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_interfaces::srv::AlimTemp>(
"alim/temp",
[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_interfaces::msg::AlimEmg>(
"alim/emg", 10,
[this](const modelec_interfaces::msg::AlimEmg::SharedPtr msg)
{
PCBEmgCallback(msg);
});
}
PCBAlimInterface::~PCBAlimInterface()
{
if (pcb_executor_)
{
pcb_executor_->cancel();
}
if (pcb_executor_thread_.joinable())
{
pcb_executor_thread_.join();
}
}
void PCBAlimInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
{
RCLCPP_DEBUG(this->get_logger(), "Received message: '%s'", msg->data.c_str());
std::vector<std::string> tokens = split(trim(msg->data), ';');
if (tokens.size() < 2)
{
RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str());
return;
}
if (tokens[0] == "SET")
{
if (startsWith(tokens[1], "OUT"))
{
bool success = true;
int value = std::stoi(tokens[3]);
ResolveGetPCBOutRequest({success, value});
}
else if (startsWith(tokens[1], "IN"))
{
bool success = true;
int value = std::stoi(tokens[3]);
ResolveGetPCBInRequest({success, value});
}
else if (tokens[1] == "BAU")
{
bool success = true;
bool activate = (tokens[3] == "1");
ResolveGetPCBBauRequest({success, activate});
}
else if (tokens[1] == "TEMP")
{
bool success = true;
int value = std::stoi(tokens[3]);
ResolveGetPCBTempRequest({success, value});
}
}
else if (tokens[0] == "OK")
{
if (startsWith(tokens[1], "OUT"))
{
bool success = true;
int value = std::stoi(tokens[3]);
ResolveSetPCBOutRequest({success, value});
}
else if (tokens[1] == "EMG")
{
bool success = true;
ResolveSetPCBEmgRequest(success);
}
}
else if (tokens[0] == "KO")
{
if (startsWith(tokens[1], "OUT"))
{
bool success = false;
int value = -1;
ResolveSetPCBOutRequest({success, value});
}
else if (tokens[1] == "EMG")
{
bool success = false;
ResolveSetPCBEmgRequest(success);
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Unknown message type: %s", tokens[0].c_str());
}
}
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_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(pcb_out_mutex_);
pcb_out_promises_.push(std::move(promise));
}
GetData(request->out, {request->type});
PCBData result = future.get();
response->success = result.success;
response->result = result.value;
}
void PCBAlimInterface::HandleSetPCBOutData(const std::shared_ptr<modelec_interfaces::srv::AlimOut::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimOut::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(pcb_out_mutex_);
pcb_out_promises_.push(std::move(promise));
}
SendOrder(request->out, {request->type, request->enable == true ? "1" : "0"});
PCBData result = future.get();
response->success = result.success;
response->result = result.value;
}
void PCBAlimInterface::HandleGetPCBInData(const std::shared_ptr<modelec_interfaces::srv::AlimIn::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimIn::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(pcb_out_mutex_);
pcb_out_promises_.push(std::move(promise));
}
GetData("IN" + request->input, {request->type});
PCBData result = future.get();
response->success = result.success;
response->result = result.value;
}
void PCBAlimInterface::HandleGetPCBBauData(const std::shared_ptr<modelec_interfaces::srv::AlimBau::Request>,
std::shared_ptr<modelec_interfaces::srv::AlimBau::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBBau> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(pcb_bau_mutex_);
pcb_bau_promises_.push(std::move(promise));
}
GetData("BAU", {"STATE"});
PCBBau result = future.get();
response->success = result.success;
response->activate = result.activate;
}
void PCBAlimInterface::HandleSetPCBEmgData(const std::shared_ptr<modelec_interfaces::srv::AlimEmg::Request> request,
std::shared_ptr<modelec_interfaces::srv::AlimEmg::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<bool> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(pcb_emg_mutex_);
pcb_emg_promises_.push(std::move(promise));
}
SendOrder("EMG", {"STATE", request->activate == true ? "1" : "0"});
response->success = future.get();
}
void PCBAlimInterface::HandleGetPCBTempData(
const std::shared_ptr<modelec_interfaces::srv::AlimTemp::Request>,
std::shared_ptr<modelec_interfaces::srv::AlimTemp::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
response->success = false;
return;
}
std::promise<PCBData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(pcb_temp_mutex_);
pcb_temp_promises_.push(std::move(promise));
}
SendOrder("TEMP", {"CELS"});
PCBData result = future.get();
response->success = result.success;
response->value = result.value;
}
void PCBAlimInterface::ResolveGetPCBOutRequest(const PCBData& value)
{
std::lock_guard<std::mutex> lock(pcb_out_mutex_);
if (!pcb_out_promises_.empty())
{
auto promise = std::move(pcb_out_promises_.front());
pcb_out_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB out data");
}
}
void PCBAlimInterface::ResolveSetPCBOutRequest(const PCBData& value)
{
std::lock_guard<std::mutex> lock(pcb_out_mutex_);
if (!pcb_out_promises_.empty())
{
auto promise = std::move(pcb_out_promises_.front());
pcb_out_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB out data");
}
}
void PCBAlimInterface::ResolveGetPCBInRequest(const PCBData& value)
{
std::lock_guard<std::mutex> lock(pcb_in_mutex_);
if (!pcb_in_promises_.empty())
{
auto promise = std::move(pcb_in_promises_.front());
pcb_in_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB in data");
}
}
void PCBAlimInterface::ResolveGetPCBBauRequest(const PCBBau& value)
{
std::lock_guard<std::mutex> lock(pcb_bau_mutex_);
if (!pcb_bau_promises_.empty())
{
auto promise = std::move(pcb_bau_promises_.front());
pcb_bau_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB bau data");
}
}
void PCBAlimInterface::ResolveSetPCBEmgRequest(bool value)
{
std::lock_guard<std::mutex> lock(pcb_emg_mutex_);
if (!pcb_emg_promises_.empty())
{
auto promise = std::move(pcb_emg_promises_.front());
pcb_emg_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB emg data");
}
}
void PCBAlimInterface::ResolveGetPCBTempRequest(const PCBData& value)
{
std::lock_guard<std::mutex> lock(pcb_temp_mutex_);
if (!pcb_temp_promises_.empty())
{
auto promise = std::move(pcb_temp_promises_.front());
pcb_temp_promises_.pop();
promise.set_value(value);
}
else
{
RCLCPP_DEBUG(this->get_logger(), "No pending request for PCB temp data");
}
}
void PCBAlimInterface::SendToPCB(const std::string& data) const
{
if (pcb_publisher_)
{
auto message = std_msgs::msg::String();
message.data = data;
pcb_publisher_->publish(message);
}
}
void PCBAlimInterface::SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data) const
{
std::string command = order + ";" + elem;
for (const auto& d : data)
{
command += ";" + d;
}
command += "\n";
SendToPCB(command);
}
void PCBAlimInterface::GetData(const std::string& elem, const std::vector<std::string>& data) const
{
SendToPCB("GET", elem, data);
}
void PCBAlimInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data) const
{
SendToPCB("SET", elem, data);
}
} // namespace Modelec
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Modelec::PCBAlimInterface>();
// Increase number of threads explicitly!
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 2 /* or more threads! */);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}

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

@@ -1,12 +1,11 @@
#include <modelec_com/pcb_odo_interface.hpp>
#include <modelec_utils/utils.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface")
PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener()
{
declare_parameter<std::string>("serial_port", "/dev/USB_ODO");
declare_parameter<int>("baudrate", 115200);
@@ -18,70 +17,7 @@ namespace Modelec
request->bauds = get_parameter("baudrate").as_int();
request->serial_port = get_parameter("serial_port").as_string();
auto client = this->create_client<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
{
if (!rclcpp::ok())
{
RCLCPP_ERROR(this->get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(this->get_logger(), "service not available, waiting again...");
}
auto result = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), result) ==
rclcpp::FutureReturnCode::SUCCESS)
{
if (auto res = result.get())
{
if (res->success)
{
pcb_callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive);
rclcpp::SubscriptionOptions options;
options.callback_group = pcb_callback_group_;
pcb_subscriber_ = this->create_subscription<std_msgs::msg::String>(
res->publisher, 10,
[this](const std_msgs::msg::String::SharedPtr msg)
{
PCBCallback(msg);
},
options);
pcb_executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
pcb_executor_->add_callback_group(pcb_callback_group_, this->get_node_base_interface());
pcb_executor_thread_ = std::thread([this]()
{
pcb_executor_->spin();
});
pcb_publisher_ = this->create_publisher<std_msgs::msg::String>(res->subscriber, 10);
isOk = true;
SetStart(true);
SetPID("THETA", 14, 0, 0);
SetPID("POS", 10, 0, 0);
SetPID("LEFT", 5, 0, 0);
SetPID("RIGHT", 5, 0, 0);
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to add serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Failed to ask for a serial listener");
}
}
else
{
RCLCPP_ERROR(this->get_logger(), "Service call failed");
}
this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN);
odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 10);
@@ -89,7 +25,7 @@ namespace Modelec
odo_get_pos_sub_ = this->create_subscription<std_msgs::msg::Empty>(
"odometry/get/pos", 30, [this](const std_msgs::msg::Empty::SharedPtr)
{
if (isOk)
if (IsOk())
{
GetPos();
}
@@ -145,29 +81,27 @@ namespace Modelec
SendOrder("START", {std::to_string(msg->data)});
}
});
SetPID("THETA", 14, 0, 0);
SetPID("POS", 10, 0, 0);
SetPID("LEFT", 5, 0, 0);
SetPID("RIGHT", 5, 0, 0);
}
PCBOdoInterface::~PCBOdoInterface()
{
SetStart(false);
if (pcb_executor_)
{
pcb_executor_->cancel();
}
if (pcb_executor_thread_.joinable())
{
pcb_executor_thread_.join();
}
}
void PCBOdoInterface::PCBCallback(const std_msgs::msg::String::SharedPtr msg)
void PCBOdoInterface::read(const std::string& msg)
{
RCLCPP_DEBUG(this->get_logger(), "Received from PCB: %s", msg->data.c_str());
std::vector<std::string> tokens = split(trim(msg->data), ';');
RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", msg.c_str());
RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str());
std::vector<std::string> tokens = split(trim(msg), ';');
if (tokens.size() < 2)
{
RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg->data.c_str());
RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg.c_str());
return;
}
@@ -254,7 +188,7 @@ namespace Modelec
}
else
{
RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg->data.c_str());
RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg.c_str());
}
}
else if (tokens[0] == "KO")
@@ -271,7 +205,7 @@ namespace Modelec
}
else
{
RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg->data.c_str());
RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg.c_str());
}
}
}
@@ -297,12 +231,10 @@ namespace Modelec
void PCBOdoInterface::SendToPCB(const std::string& data)
{
if (pcb_publisher_)
if (IsOk())
{
RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str());
auto message = std_msgs::msg::String();
message.data = data;
pcb_publisher_->publish(message);
this->write(data);
}
}
@@ -449,17 +381,18 @@ namespace Modelec
}
} // Modelec
#ifndef MODELEC_COM_TESTING
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Modelec::PCBOdoInterface>();
// Increase number of threads explicitly!
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 2 /* or more threads! */);
rclcpp::ExecutorOptions(), 2);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}
#endif

398
src/modelec_com/src/pcb_odo_interface.new.cpp
View File

@@ -1,398 +0,0 @@
#include <modelec_com/pcb_odo_interface.new.hpp>
#include <modelec_utils/utils.hpp>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface"), SerialListener()
{
declare_parameter<std::string>("serial_port", "/dev/USB_ODO");
declare_parameter<int>("baudrate", 115200);
declare_parameter<std::string>("name", "pcb_odo");
// Service to create a new serial listener
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = get_parameter("name").as_string();
request->bauds = get_parameter("baudrate").as_int();
request->serial_port = get_parameter("serial_port").as_string();
this->open(request->name, request->bauds, request->serial_port, MAX_MESSAGE_LEN);
odo_pos_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 10);
odo_get_pos_sub_ = this->create_subscription<std_msgs::msg::Empty>(
"odometry/get/pos", 30, [this](const std_msgs::msg::Empty::SharedPtr)
{
if (IsOk())
{
GetPos();
}
});
odo_speed_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometrySpeed>(
"odometry/speed", 10);
odo_tof_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryToF>(
"odometry/tof", 10);
odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryWaypoint>(
"odometry/waypoint_reach", 10);
odo_pid_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryPid>(
"odometry/get_pid", 10);
odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
"odometry/add_waypoint", 30,
[this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
{
AddWaypointCallback(msg);
});
odo_add_waypoints_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryWaypoints>(
"odometry/add_waypoints", 30,
[this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
{
AddWaypointsCallback(msg);
});
odo_set_pos_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryPos>(
"odometry/set_position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
SetPosCallback(msg);
});
odo_set_pid_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryPid>(
"odometry/set_pid", 10,
[this](const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
{
SetPIDCallback(msg);
});
start_odo_sub_ = this->create_subscription<std_msgs::msg::Bool>(
"odometry/start", 10,
[this](const std_msgs::msg::Bool::SharedPtr msg)
{
if (msg->data != start_odo_)
{
start_odo_ = msg->data;
SendOrder("START", {std::to_string(msg->data)});
}
});
SetPID("THETA", 14, 0, 0);
SetPID("POS", 10, 0, 0);
SetPID("LEFT", 5, 0, 0);
SetPID("RIGHT", 5, 0, 0);
}
PCBOdoInterface::~PCBOdoInterface()
{
SetStart(false);
}
void PCBOdoInterface::read(const std::string& msg)
{
RCLCPP_INFO_ONCE(this->get_logger(), "Received from PCB: %s", msg.c_str());
RCLCPP_DEBUG_SKIPFIRST(this->get_logger(), "Received from PCB: %s", msg.c_str());
std::vector<std::string> tokens = split(trim(msg), ';');
if (tokens.size() < 2)
{
RCLCPP_ERROR(this->get_logger(), "Invalid message format: %s", msg.c_str());
return;
}
if (tokens[0] == "SET")
{
if (tokens[1] == "POS")
{
long x = std::stol(tokens[2]);
long y = std::stol(tokens[3]);
double theta = std::stod(tokens[4]);
auto message = modelec_interfaces::msg::OdometryPos();
message.x = x;
message.y = y;
message.theta = theta;
if (odo_pos_publisher_) {
odo_pos_publisher_->publish(message);
}
}
else if (tokens[1] == "SPEED")
{
long x = std::stol(tokens[2]);
long y = std::stol(tokens[3]);
double theta = std::stod(tokens[4]);
auto message = modelec_interfaces::msg::OdometrySpeed();
message.x = x;
message.y = y;
message.theta = theta;
odo_speed_publisher_->publish(message);
}
else if (tokens[1] == "DIST")
{
int n = std::stoi(tokens[2]);
long dist = std::stol(tokens[3]);
auto message = modelec_interfaces::msg::OdometryToF();
message.n = n;
message.distance = dist;
odo_tof_publisher_->publish(message);
}
else if (tokens[1] == "WAYPOINT")
{
int id = std::stoi(tokens[2]);
auto message = modelec_interfaces::msg::OdometryWaypoint();
message.id = id;
odo_waypoint_reach_publisher_->publish(message);
}
else if (tokens[1] == "PID")
{
std::string name = tokens[2];
float p = std::stof(tokens[3]);
float i = std::stof(tokens[4]);
float d = std::stof(tokens[5]);
auto message = modelec_interfaces::msg::OdometryPid();
message.name = name;
message.p = p;
message.i = i;
message.d = d;
odo_pid_publisher_->publish(message);
}
}
else if (tokens[0] == "OK")
{
if (tokens[1] == "START")
{
start_odo_ = tokens[2] == "1";
}
else if (tokens[1] == "WAYPOINT")
{
}
else if (tokens[1] == "PID")
{
}
else if (tokens[1] == "POS")
{
}
else
{
RCLCPP_INFO(this->get_logger(), "PCB response: %s", msg.c_str());
}
}
else if (tokens[0] == "KO")
{
if (tokens[1] == "START")
{
}
else if (tokens[1] == "WAYPOINT")
{
}
else if (tokens[1] == "PID")
{
}
else
{
RCLCPP_WARN(this->get_logger(), "PCB error: %s", msg.c_str());
}
}
}
void PCBOdoInterface::AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) {
AddWaypoints(msg);
}
void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
{
AddWaypoint(msg);
}
void PCBOdoInterface::SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
SetRobotPos(msg);
}
void PCBOdoInterface::SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
{
SetPID(msg);
}
void PCBOdoInterface::SendToPCB(const std::string& data)
{
if (IsOk())
{
RCLCPP_DEBUG(this->get_logger(), "Sending to PCB: %s", data.c_str());
this->write(data);
}
}
void PCBOdoInterface::SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data)
{
std::string command = order + ";" + elem;
for (const auto& d : data)
{
command += ";" + d;
}
command += "\n";
SendToPCB(command);
}
void PCBOdoInterface::GetData(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("GET", elem, data);
}
void PCBOdoInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("SET", elem, data);
}
void PCBOdoInterface::GetPos()
{
GetData("POS");
}
void PCBOdoInterface::GetSpeed()
{
GetData("SPEED");
}
void PCBOdoInterface::GetToF(const int& tof)
{
GetData("DIST", {std::to_string(tof)});
}
void PCBOdoInterface::SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
SetRobotPos(msg->x, msg->y, msg->theta);
}
void PCBOdoInterface::SetRobotPos(const long x, const long y, const double theta)
{
std::vector<std::string> data = {
std::to_string(x),
std::to_string(y),
std::to_string(theta)
};
SendOrder("POS", data);
}
void PCBOdoInterface::AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg)
{
if (!start_odo_)
{
SendOrder("START", {std::to_string(true)});
}
std::vector<std::string> data;
for (const auto& wp : msg->waypoints)
{
data.push_back(std::to_string(wp.id));
data.push_back(std::to_string(wp.is_end));
data.push_back(std::to_string(wp.x));
data.push_back(std::to_string(wp.y));
data.push_back(std::to_string(wp.theta));
}
SendOrder("WAYPOINT", data);
}
void PCBOdoInterface::AddWaypoint(
const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg)
{
AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta);
}
void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y,
const double theta)
{
if (!start_odo_)
{
SendOrder("START", {std::to_string(true)});
}
std::vector<std::string> data = {
std::to_string(index),
std::to_string(IsStopPoint),
std::to_string(x),
std::to_string(y),
std::to_string(theta)
};
SendOrder("WAYPOINT", data);
}
void PCBOdoInterface::SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg)
{
SetStart(msg->start);
}
void PCBOdoInterface::SetStart(bool start)
{
SendOrder("START", {std::to_string(start)});
}
void PCBOdoInterface::GetPID()
{
GetData("PID");
}
void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
{
SetPID(msg->name, msg->p, msg->i, msg->d);
}
void PCBOdoInterface::SetPID(std::string name, float p, float i, float d, std::optional<float> min, std::optional<float> max)
{
std::vector<std::string> data = {
name,
std::to_string(p),
std::to_string(i),
std::to_string(d)
};
if (min.has_value())
{
data.push_back(std::to_string(min.value()));
}
if (max.has_value())
{
data.push_back(std::to_string(max.value()));
}
SendOrder("PID", data);
}
} // Modelec
#ifndef MODELEC_COM_TESTING
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<Modelec::PCBOdoInterface>();
rclcpp::executors::MultiThreadedExecutor executor(
rclcpp::ExecutorOptions(), 2);
executor.add_node(node);
executor.spin();
rclcpp::shutdown();
return 0;
}
#endif

4
src/modelec_core/launch/modelec.launch.py
View File

@@ -96,7 +96,7 @@ def generate_launch_description():
Node(package='modelec_com', executable='serial_listener', name='serial_listener'),
Node(
package='modelec_com',
executable='pcb_odo_interface_new',
executable='pcb_odo_interface',
name='pcb_odo_interface',
parameters=[{
'serial_port': "/dev/USB_ODO",
@@ -106,7 +106,7 @@ def generate_launch_description():
),
Node(
package='modelec_com',
executable='pcb_action_interface_new',
executable='pcb_action_interface',
name='pcb_action_interface',
parameters=[{
'serial_port': "/dev/USB_ACTION",