diff --git a/install.sh b/install.sh index 08fba98..034ee44 100755 --- a/install.sh +++ b/install.sh @@ -21,24 +21,14 @@ sudo apt upgrade -y sudo apt install ros-dev-tools ros-jazzy-desktop -y +sudo apt-get install qt6-base-dev qt6-svg-dev libxml2-dev socat -y + git submodule init git submodule update -cd WiringPi || exit 1 - -./build debian -mv ./debian-template/wiringpi_*.deb . - -# install it -sudo apt install ./wiringpi_*.deb -y - -cd .. - -sudo apt-get install qt6-base-dev qt6-svg-dev libxml2-dev socat -y - -echo "source /opt/ros/jazzy/setup.bash" >> ~/.bashrc -echo "source ~/modelec-marcel-ROS/install/setup.bash" >> ~/.bashrc -echo "export RMW_IMPLEMENTATION=rmw_fastrtps_cpp +echo "source /opt/ros/jazzy/setup.bash +source ~/modelec-marcel-ROS/install/setup.bash +export RMW_IMPLEMENTATION=rmw_fastrtps_cpp export FASTRTPS_DEFAULT_PROFILES_FILE=~/modelec-marcel-ROS/fastdds_setup.xml export ROS_DOMAIN_ID=128" >> ~/.bashrc diff --git a/simulated_pcb/action.py b/simulated_pcb/action.py index d89a073..39b93b4 100644 --- a/simulated_pcb/action.py +++ b/simulated_pcb/action.py @@ -122,7 +122,7 @@ class SimulatedPCB: if __name__ == '__main__': parser = argparse.ArgumentParser(description="Simulated PCB") - parser.add_argument('--port', type=str, default='/dev/pts/6', help='Serial port to use') + parser.add_argument('--port', type=str, default='/tmp/USB_ACTION_SIM', help='Serial port to use') args = parser.parse_args() sim = None @@ -132,3 +132,8 @@ if __name__ == '__main__': if sim: sim.stop() print("Simulation stopped") + +# socat -d -d pty,raw,echo=0,link=/tmp/MARCEL_ACTION_SIM pty,raw,echo=0,link=/tmp/MARCEL_ACTION +# python3 simulated_pcb/action.py --port /tmp/MARCEL_ACTION_SIM +# socat -d -d pty,raw,echo=0,link=/tmp/ENEMY_ACTION_SIM pty,raw,echo=0,link=/tmp/ENEMY_ACTION +# python3 simulated_pcb/action.py --port /tmp/ENEMY_ACTION_SIM \ No newline at end of file diff --git a/simulated_pcb/odo.py b/simulated_pcb/odo.py index a63e62a..a9bec9a 100644 --- a/simulated_pcb/odo.py +++ b/simulated_pcb/odo.py @@ -124,20 +124,24 @@ class SimulatedPCB: print(f'[TX] OK;START;1') self.ser.write(f'OK;START;1\n'.encode()) - elif msg.startswith("SET;WAYPOINT"): + elif msg.startswith("SET;WAYPOINTS;"): parts = msg.split(';') - if len(parts) >= 7: - wp = { - 'id': int(parts[2]), - 'type': int(parts[3]), - 'x': float(parts[4]), - 'y': float(parts[5]), - 'theta': float(parts[6]) - } - self.waypoints[wp['id']] = wp - if wp['id'] not in self.waypoint_order: - self.waypoint_order.append(wp['id']) - self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode()) + if len(parts) / 7 >= 1: + self.waypoints.clear() + self.waypoint_order.clear() + for i in range(2, len(parts), 5): + wp = { + 'id': int(parts[i]), + 'type': int(parts[i + 1]), + 'x': float(parts[i + 2]), + 'y': float(parts[i + 3]), + 'theta': float(parts[i + 4]) + } + self.waypoints[wp['id']] = wp + if wp['id'] not in self.waypoint_order: + self.waypoint_order.append(wp['id']) + self.ser.write(f'OK;WAYPOINT;{wp["id"]}\n'.encode()) + self.ser.write(b'OK;WAYPOINTS;1\n') elif msg.startswith("SET;POS"): parts = msg.split(';') diff --git a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp index 31dfb8e..a5d68cb 100644 --- a/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp +++ b/src/modelec_com/include/modelec_com/pcb_odo_interface.hpp @@ -11,8 +11,8 @@ #include #include #include -#include -#include +#include +#include #include #include @@ -64,83 +64,22 @@ namespace Modelec rclcpp::Subscription::SharedPtr odo_get_pos_sub_; rclcpp::Publisher::SharedPtr odo_speed_publisher_; rclcpp::Publisher::SharedPtr odo_tof_publisher_; - rclcpp::Publisher::SharedPtr odo_waypoint_reach_publisher_; + rclcpp::Publisher::SharedPtr odo_waypoint_reach_publisher_; rclcpp::Publisher::SharedPtr odo_pid_publisher_; - rclcpp::Subscription::SharedPtr odo_add_waypoint_subscriber_; + rclcpp::Subscription::SharedPtr odo_add_waypoint_subscriber_; + rclcpp::Subscription::SharedPtr odo_add_waypoints_subscriber_; rclcpp::Subscription::SharedPtr odo_set_pos_subscriber_; rclcpp::Subscription::SharedPtr odo_set_pid_subscriber_; - void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg); + 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::Service::SharedPtr get_tof_service_; - rclcpp::Service::SharedPtr get_speed_service_; - rclcpp::Service::SharedPtr get_position_service_; - rclcpp::Service::SharedPtr set_start_service_; - rclcpp::Service::SharedPtr get_pid_service_; - rclcpp::Service::SharedPtr set_pid_service_; - rclcpp::Service::SharedPtr add_waypoint_service_; - rclcpp::Subscription::SharedPtr start_odo_sub_; bool start_odo_ = false; - // rclcpp::TimerBase::SharedPtr odo_get_pos_timer_; - - // Promises and mutexes to synchronize service responses asynchronously - std::queue> tof_promises_; - std::mutex tof_mutex_; - - std::queue> speed_promises_; - std::mutex speed_mutex_; - - std::queue> pos_promises_; - std::mutex pos_mutex_; - - std::queue> start_promises_; - std::mutex start_mutex_; - - std::queue> get_pid_promises_; - std::mutex get_pid_mutex_; - - std::queue> set_pid_promises_; - std::mutex set_pid_mutex_; - - std::queue> add_waypoint_promises_; - std::mutex add_waypoint_mutex_; - - // Service handlers using async wait with promises - void HandleGetTof(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetSpeed(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPosition(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetStart(const std::shared_ptr request, - std::shared_ptr response); - - void HandleGetPID(const std::shared_ptr request, - std::shared_ptr response); - - void HandleSetPID(const std::shared_ptr request, - std::shared_ptr response); - - void HandleAddWaypoint(const std::shared_ptr request, - std::shared_ptr response); - - // Resolving methods called by subscriber callback - void ResolveToFRequest(long distance); - void ResolveSpeedRequest(const OdometryData& speed); - void ResolvePositionRequest(const OdometryData& position); - void ResolveStartRequest(bool start); - void ResolveGetPIDRequest(const PIDData& pid); - void ResolveSetPIDRequest(bool success); - void ResolveAddWaypointRequest(bool success); - int timeout_ms = 1000; int attempt = 5; @@ -161,7 +100,8 @@ namespace Modelec void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); void SetRobotPos(long x, long y, double theta); - void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg); + 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); @@ -169,6 +109,6 @@ namespace Modelec void GetPID(); void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg); - void SetPID(float p, float i, float d); + void SetPID(std::string name, float p, float i, float d); }; } // namespace Modelec diff --git a/src/modelec_com/src/pcb_alim_interface.cpp b/src/modelec_com/src/pcb_alim_interface.cpp index 33e7f0c..0c325d4 100644 --- a/src/modelec_com/src/pcb_alim_interface.cpp +++ b/src/modelec_com/src/pcb_alim_interface.cpp @@ -8,16 +8,18 @@ namespace Modelec PCBAlimInterface::PCBAlimInterface() : Node("pcb_alim_interface") { // Service to create a new serial listener - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Config::load(config_path)) - { - RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); - } + declare_parameter("serial_port", "/dev/ttyUSB0"); + declare_parameter("baudrate", 115200); + declare_parameter("name", "pcb_action"); auto request = std::make_shared(); - request->name = Config::get("config.usb.pcb.pcb_alim.name", "pcb_odo"); - request->bauds = Config::get("config.usb.pcb.pcb_alim.baudrate", 115200); - request->serial_port = Config::get("config.usb.pcb.pcb_alim.port", "/dev/ttyUSB0"); + 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("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) diff --git a/src/modelec_com/src/pcb_odo_interface.cpp b/src/modelec_com/src/pcb_odo_interface.cpp index f8bc0b1..bbaec9c 100644 --- a/src/modelec_com/src/pcb_odo_interface.cpp +++ b/src/modelec_com/src/pcb_odo_interface.cpp @@ -8,17 +8,15 @@ namespace Modelec { PCBOdoInterface::PCBOdoInterface() : Node("pcb_odo_interface") { - std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml"; - if (!Config::load(config_path)) - { - RCLCPP_ERROR(get_logger(), "Failed to load config file: %s", config_path.c_str()); - } + declare_parameter("serial_port", "/dev/USB_ODO"); + declare_parameter("baudrate", 115200); + declare_parameter("name", "pcb_action"); // Service to create a new serial listener auto request = std::make_shared(); - request->name = Config::get("config.usb.pcb.pcb_odo.name", "pcb_odo"); - request->bauds = Config::get("config.usb.pcb.pcb_odo.baudrate", 115200); - request->serial_port = Config::get("config.usb.pcb.pcb_odo.port", "/dev/ttyUSB0"); + request->name = get_parameter("name").as_string(); + request->bauds = get_parameter("baudrate").as_int(); + request->serial_port = get_parameter("serial_port").as_string(); auto client = this->create_client("add_serial_listener"); while (!client->wait_for_service(std::chrono::seconds(1))) @@ -96,21 +94,26 @@ namespace Modelec odo_tof_publisher_ = this->create_publisher( "odometry/tof", 10); - odo_waypoint_reach_publisher_ = this->create_publisher( + odo_waypoint_reach_publisher_ = this->create_publisher( "odometry/waypoint_reach", 10); odo_pid_publisher_ = this->create_publisher( "odometry/get_pid", 10); - odo_add_waypoint_subscriber_ = this->create_subscription( + odo_add_waypoint_subscriber_ = this->create_subscription( "odometry/add_waypoint", 30, - [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) + [this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg) { - RCLCPP_INFO(this->get_logger(), "Received add waypoint request: %d %s %d %d %f", - msg->id, msg->is_end ? "end" : "not end", msg->x, msg->y, msg->theta); AddWaypointCallback(msg); }); + odo_add_waypoints_subscriber_ = this->create_subscription( + "odometry/add_waypoint", 30, + [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) + { + AddWaypointsCallback(msg); + }); + odo_set_pos_subscriber_ = this->create_subscription( "odometry/set_position", 10, [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) @@ -125,63 +128,6 @@ namespace Modelec SetPIDCallback(msg); }); - // Services - get_tof_service_ = create_service( - "odometry/tof", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleGetTof(request, response); - }); - - get_speed_service_ = create_service( - "odometry/speed", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleGetSpeed(request, response); - }); - - get_position_service_ = create_service( - "odometry/get_position", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleGetPosition(request, response); - }); - - set_start_service_ = create_service( - "odometry/start", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleGetStart(request, response); - }); - - get_pid_service_ = create_service( - "odometry/get_pid", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleGetPID(request, response); - }); - - set_pid_service_ = create_service( - "odometry/set_pid", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleSetPID(request, response); - }); - - add_waypoint_service_ = create_service( - "odometry/add_waypoint", - [this](const std::shared_ptr request, - std::shared_ptr response) - { - HandleAddWaypoint(request, response); - }); - start_odo_sub_ = this->create_subscription( "odometry/start", 10, [this](const std_msgs::msg::Bool::SharedPtr msg) @@ -193,18 +139,6 @@ namespace Modelec SendOrder("START", {std::to_string(msg->data)}); } }); - - /*odo_get_pos_timer_ = this->create_wall_timer( - std::chrono::milliseconds(500), - [this]() - { - if (isOk && start_odo_) - { - RCLCPP_INFO(this->get_logger(), "Requesting position from PCB"); - - GetPos(); - } - });*/ } PCBOdoInterface::~PCBOdoInterface() @@ -243,8 +177,6 @@ namespace Modelec message.theta = theta; odo_pos_publisher_->publish(message); - - ResolvePositionRequest({x, y, theta}); } else if (tokens[1] == "SPEED") { @@ -258,7 +190,6 @@ namespace Modelec message.theta = theta; odo_speed_publisher_->publish(message); - ResolveSpeedRequest({x, y, theta}); } else if (tokens[1] == "DIST") { @@ -270,53 +201,46 @@ namespace Modelec message.distance = dist; odo_tof_publisher_->publish(message); - ResolveToFRequest(dist); } else if (tokens[1] == "WAYPOINT") { int id = std::stoi(tokens[2]); - auto message = modelec_interfaces::msg::OdometryWaypointReach(); + auto message = modelec_interfaces::msg::OdometryWaypoint(); message.id = id; odo_waypoint_reach_publisher_->publish(message); } else if (tokens[1] == "PID") { - float p = std::stof(tokens[2]); - float i = std::stof(tokens[3]); - float d = std::stof(tokens[4]); + 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); - ResolveGetPIDRequest({p, i, d}); } } else if (tokens[0] == "OK") { if (tokens[1] == "START") { - // bool start = std::stoi(tokens[2]); - ResolveStartRequest(true); } else if (tokens[1] == "WAYPOINT") { - bool success = true; - ResolveAddWaypointRequest(success); } else if (tokens[1] == "PID") { - bool success = true; - ResolveSetPIDRequest(success); } else if (tokens[1] == "POS") { - // position set } else { @@ -327,19 +251,13 @@ namespace Modelec { if (tokens[1] == "START") { - bool start = false; - ResolveStartRequest(start); } else if (tokens[1] == "WAYPOINT") { - bool success = false; - ResolveAddWaypointRequest(success); } else if (tokens[1] == "PID") { - bool success = false; - ResolveSetPIDRequest(success); } else { @@ -348,7 +266,11 @@ namespace Modelec } } - void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) + void PCBOdoInterface::AddWaypointsCallback(const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) { + AddWaypoints(msg); + } + + void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg) { AddWaypoint(msg); } @@ -363,281 +285,6 @@ namespace Modelec SetPID(msg); } - void PCBOdoInterface::HandleGetTof( - const std::shared_ptr request, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(tof_mutex_); - tof_promises_.push(std::move(promise)); - } - - GetToF(request->n); - - response->distance = future.get(); - } - - void PCBOdoInterface::HandleGetSpeed( - const std::shared_ptr, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(speed_mutex_); - speed_promises_.push(std::move(promise)); - } - - GetSpeed(); - - OdometryData result = future.get(); - response->x = result.x; - response->y = result.y; - response->theta = result.theta; - } - - void PCBOdoInterface::HandleGetPosition( - const std::shared_ptr, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(pos_mutex_); - pos_promises_.push(std::move(promise)); - } - - GetPos(); - - OdometryData result = future.get(); - response->x = result.x; - response->y = result.y; - response->theta = result.theta; - } - - void PCBOdoInterface::HandleGetStart(const std::shared_ptr request, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(start_mutex_); - start_promises_.push(std::move(promise)); - } - - SetStart(request->start); - response->success = future.get(); - } - - void PCBOdoInterface::HandleGetPID(const std::shared_ptr, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(get_pid_mutex_); - get_pid_promises_.push(std::move(promise)); - } - - GetPID(); - - PIDData result = future.get(); - - response->p = result.p; - response->i = result.i; - response->d = result.d; - } - - void PCBOdoInterface::HandleSetPID(const std::shared_ptr request, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(set_pid_mutex_); - set_pid_promises_.push(std::move(promise)); - } - - SetPID(request->p, request->i, request->d); - - bool result = future.get(); - response->success = result; - } - - void PCBOdoInterface::HandleAddWaypoint( - const std::shared_ptr request, - std::shared_ptr response) - { - if (!isOk) - { - RCLCPP_ERROR(get_logger(), "PCB not initialized"); - response->success = false; - return; - } - - std::promise promise; - auto future = promise.get_future(); - - { - std::lock_guard lock(add_waypoint_mutex_); - add_waypoint_promises_.push(std::move(promise)); - } - AddWaypoint(request->id, request->is_end, request->x, request->y, request->theta); - bool result = future.get(); - response->success = result; - } - - void PCBOdoInterface::ResolveToFRequest(const long distance) - { - std::lock_guard lock(tof_mutex_); - if (!tof_promises_.empty()) - { - auto promise = std::move(tof_promises_.front()); - tof_promises_.pop(); - promise.set_value(distance); - } - else - { - RCLCPP_DEBUG(get_logger(), "No pending ToF request to resolve."); - } - } - - void PCBOdoInterface::ResolveSpeedRequest(const OdometryData& speed) - { - std::lock_guard lock(speed_mutex_); - if (!speed_promises_.empty()) - { - auto promise = std::move(speed_promises_.front()); - speed_promises_.pop(); - promise.set_value(speed); - } - else - { - RCLCPP_DEBUG(get_logger(), "No pending Speed request to resolve."); - } - } - - void PCBOdoInterface::ResolvePositionRequest(const OdometryData& position) - { - std::lock_guard lock(pos_mutex_); - if (!pos_promises_.empty()) - { - auto promise = std::move(pos_promises_.front()); - pos_promises_.pop(); - promise.set_value(position); - } - else - { - RCLCPP_DEBUG(get_logger(), "No pending Position request to resolve."); - } - } - - void PCBOdoInterface::ResolveStartRequest(bool start) - { - std::lock_guard lock(start_mutex_); - if (!start_promises_.empty()) - { - auto promise = std::move(start_promises_.front()); - start_promises_.pop(); - promise.set_value(start); - } - else - { - RCLCPP_DEBUG(get_logger(), "No pending Start request to resolve."); - } - } - - void PCBOdoInterface::ResolveGetPIDRequest(const PIDData& pid) - { - std::lock_guard lock(get_pid_mutex_); - if (!get_pid_promises_.empty()) - { - auto promise = std::move(get_pid_promises_.front()); - get_pid_promises_.pop(); - promise.set_value(pid); - } - else - { - RCLCPP_DEBUG(get_logger(), "No pending PID request to resolve."); - } - } - - void PCBOdoInterface::ResolveSetPIDRequest(bool success) - { - std::lock_guard lock(set_pid_mutex_); - if (!set_pid_promises_.empty()) - { - auto promise = std::move(set_pid_promises_.front()); - set_pid_promises_.pop(); - promise.set_value(success); - } - else - { - RCLCPP_DEBUG(get_logger(), "No pending Set PID request to resolve."); - } - } - - void PCBOdoInterface::ResolveAddWaypointRequest(bool success) - { - std::lock_guard lock(add_waypoint_mutex_); - if (!add_waypoint_promises_.empty()) - { - auto promise = std::move(add_waypoint_promises_.front()); - add_waypoint_promises_.pop(); - promise.set_value(success); - } - else - { - RCLCPP_DEBUG(get_logger(), "No pending Add Waypoint request to resolve."); - } - } - void PCBOdoInterface::SendToPCB(const std::string& data) { if (pcb_publisher_) @@ -703,8 +350,30 @@ namespace Modelec SendOrder("POS", data); } + void PCBOdoInterface::AddWaypoints(modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) + { + if (!start_odo_) + { + SendOrder("START", {std::to_string(true)}); + start_odo_ = true; + } + + std::vector data(msg->waypoints.size() * 5); + + 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::OdometryAddWaypoint::SharedPtr msg) + const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg) { AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta); } @@ -746,12 +415,13 @@ namespace Modelec void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) { - SetPID(msg->p, msg->i, msg->d); + SetPID(msg->name, msg->p, msg->i, msg->d); } - void PCBOdoInterface::SetPID(float p, float i, float d) + void PCBOdoInterface::SetPID(std::string name, float p, float i, float d) { std::vector data = { + name, std::to_string(p), std::to_string(i), std::to_string(d) diff --git a/src/modelec_core/CMakeLists.txt b/src/modelec_core/CMakeLists.txt index 5cceb3c..2f02237 100644 --- a/src/modelec_core/CMakeLists.txt +++ b/src/modelec_core/CMakeLists.txt @@ -17,14 +17,6 @@ find_package(Boost REQUIRED COMPONENTS system) find_package(modelec_interfaces REQUIRED) find_package(modelec_utils REQUIRED) -add_executable(gamecontroller_listener src/gamecontroller_listener.cpp) -ament_target_dependencies(gamecontroller_listener rclcpp std_msgs sensor_msgs modelec_interfaces) -target_link_libraries(gamecontroller_listener modelec_utils::utils) -target_include_directories(gamecontroller_listener PUBLIC - $ - $ -) - add_executable(speed_result src/speed_result.cpp) ament_target_dependencies(speed_result rclcpp std_msgs sensor_msgs modelec_interfaces) target_link_libraries(speed_result modelec_utils::utils) @@ -43,7 +35,6 @@ endif() # Install targets install(TARGETS - gamecontroller_listener speed_result DESTINATION lib/${PROJECT_NAME} ) diff --git a/src/modelec_core/include/modelec/gamecontroller_listener.hpp b/src/modelec_core/include/modelec/gamecontroller_listener.hpp deleted file mode 100644 index d6b513a..0000000 --- a/src/modelec_core/include/modelec/gamecontroller_listener.hpp +++ /dev/null @@ -1,65 +0,0 @@ -#pragma once - -#include "modelec_interfaces/msg/pca9685_servo.hpp" -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace Modelec { - class ControllerListener : public rclcpp::Node - { - using ServoMode = modelec_interfaces::msg::ServoMode; - - std::array pinces = { - ServoMode::PINCE_CLOSED, - ServoMode::PINCE_CLOSED, - ServoMode::PINCE_CLOSED, - }; - - int arm = ServoMode::ARM_BOTTOM; - - struct SolarPannelServo { - int pin; - float startAngle; - float endAngle; - }; - - std::array solarPannelServos = { - { { 6, 16.0f, 40.0f }, { 7, 25.0f, 3.0f } } - }; - - public: - ControllerListener(); - - private: - void joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg); - - rclcpp::Subscription::SharedPtr subscription_; - rclcpp::Publisher::SharedPtr servo_publisher_; - rclcpp::Publisher::SharedPtr odometry_publisher_; - rclcpp::Publisher::SharedPtr clear_pca_publisher_; - rclcpp::Publisher::SharedPtr pca9685_publisher_; - rclcpp::Client::SharedPtr client_; - - void CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg); - void CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg); - - bool button_2_was_pressed = false; - bool button_3_was_pressed = false; - bool button_1_was_pressed = false; - bool button_0_was_pressed = false; - bool button_4_was_pressed = false; - bool button_5_was_pressed = false; - - int last_speed = 0; - int last_rotation = 0; - float last_solar_1_angle = 0; - float last_solar_2_angle = 0; - }; -} diff --git a/src/modelec_core/src/gamecontroller_listener.cpp b/src/modelec_core/src/gamecontroller_listener.cpp deleted file mode 100644 index dc50903..0000000 --- a/src/modelec_core/src/gamecontroller_listener.cpp +++ /dev/null @@ -1,310 +0,0 @@ -#include "modelec/gamecontroller_listener.hpp" -#include "modelec_utils/utils.hpp" -#include - -namespace Modelec -{ - ControllerListener::ControllerListener() : Node("controller_listener") - { - // Subscribe to the 'joy' topic - subscription_ = this->create_subscription( - "joy", 10, - std::bind(&ControllerListener::joy_callback, this, std::placeholders::_1)); - - servo_publisher_ = this->create_publisher("arm_control", 10); - - // Service to create a new serial listener - auto request = std::make_shared(); - request->name = "odometry"; - request->bauds = 115200; - request->serial_port = "/dev/ttyACM0"; - auto client = this->create_client("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) - { - RCLCPP_INFO(this->get_logger(), "Serial listener created"); - - odometry_publisher_ = this->create_publisher(result.get()->subscriber, 10); - - clear_pca_publisher_ = this->create_publisher("clear_pca9685", 10); - - pca9685_publisher_ = this->create_publisher( - "servo_control", 10); - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to create serial listener"); - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "Failed to get result from service"); - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "Service call failed"); - } - - client_ = this->create_client("add_servo"); - - // ensure the server is up - 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..."); - } - - for (auto servo : solarPannelServos) - { - auto request = std::make_shared(); - request->pin = servo.pin; - auto res = client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) == - rclcpp::FutureReturnCode::SUCCESS) - { - if (!res.get()->success) - { - RCLCPP_ERROR(this->get_logger(), "Failed to add servo on pin %d", servo.pin); - } - else - { - RCLCPP_INFO(this->get_logger(), "Added servo on pin %d", servo.pin); - } - } - else - { - RCLCPP_ERROR(this->get_logger(), "Service call failed"); - } - } - } - - void ControllerListener::joy_callback(const sensor_msgs::msg::Joy::SharedPtr msg) - { - // to check mapping : https://index.ros.org//p/joy/ - // bon en fait la doc dit de la merde pour voir les bon checker directement en loggant le topic joy - CheckButton(msg); - CheckAxis(msg); - } - - void ControllerListener::CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg) - { - if (msg->buttons[2] == 1) - { - if (button_2_was_pressed) - { - return; - } - auto message = ServoMode(); - message.pin = 0; - if (pinces[0] == ServoMode::PINCE_CLOSED) - { - pinces[0] = ServoMode::PINCE_OPEN; - } - else - { - pinces[0] = ServoMode::PINCE_CLOSED; - } - message.mode = pinces[0]; - servo_publisher_->publish(message); - button_2_was_pressed = true; - } - else - { - button_2_was_pressed = false; - } - if (msg->buttons[3] == 1) - { - if (button_3_was_pressed) - { - return; - } - auto message = ServoMode(); - message.pin = 1; - if (pinces[1] == ServoMode::PINCE_CLOSED) - { - pinces[1] = ServoMode::PINCE_OPEN; - } - else - { - pinces[1] = ServoMode::PINCE_CLOSED; - } - message.mode = pinces[1]; - servo_publisher_->publish(message); - button_3_was_pressed = true; - } - else - { - button_3_was_pressed = false; - } - if (msg->buttons[1] == 1) - { - if (button_1_was_pressed) - { - return; - } - auto message = ServoMode(); - message.pin = 2; - if (pinces[2] == ServoMode::PINCE_CLOSED) - { - pinces[2] = ServoMode::PINCE_OPEN; - } - else - { - pinces[2] = ServoMode::PINCE_CLOSED; - } - message.mode = pinces[2]; - servo_publisher_->publish(message); - button_1_was_pressed = true; - } - else - { - button_1_was_pressed = false; - } - - // arm - if (msg->buttons[0] == 1) - { - if (button_0_was_pressed) - { - return; - } - auto message = ServoMode(); - if (arm == ServoMode::ARM_BOTTOM) - { - arm = ServoMode::ARM_TOP; - } - else - { - arm = ServoMode::ARM_BOTTOM; - } - message.mode = arm; - message.is_arm = true; - servo_publisher_->publish(message); - button_0_was_pressed = true; - } - else - { - button_0_was_pressed = false; - } - - if (msg->buttons[4]) - { - if (button_4_was_pressed) - { - return; - } - auto message = std_msgs::msg::String(); - message.data = "W"; - odometry_publisher_->publish(message); - button_4_was_pressed = true; - } - else - { - button_4_was_pressed = false; - } - - if (msg->buttons[5]) - { - if (button_5_was_pressed) - { - return; - } - clear_pca_publisher_->publish(std_msgs::msg::Empty()); - button_5_was_pressed = true; - } - else - { - button_5_was_pressed = false; - } - } - - void ControllerListener::CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg) - { - auto message = std_msgs::msg::String(); - int speed = 0; - if (msg->axes[1] < 0.1 && msg->axes[1] > -0.1) - { - speed = 0; - } - else - { - speed = static_cast(Modelec::mapValue(static_cast(msg->axes[1]), -1.0f, 1.0f, -310.0f, 310.0f)); - } - - if (speed != last_speed) - { - message.data = "V " + std::to_string(speed); - odometry_publisher_->publish(message); - last_speed = speed; - } - - int rotation = 0; - if (msg->axes[3] < 0.1 && msg->axes[3] > -0.1) - { - rotation = 0; - } - else - { - rotation = static_cast(Modelec::mapValue(static_cast(-msg->axes[3]), -1.0f, 1.0f, -310.0f, - 310.0f)); - } - - if (rotation != last_rotation) - { - message.data = "R " + std::to_string(rotation); - odometry_publisher_->publish(message); - last_rotation = rotation; - } - - if (msg->axes[2] != last_solar_1_angle) - { - int solarPannelAngle = static_cast(Modelec::mapValue(static_cast(msg->axes[2]), -1.0f, 1.0f, - solarPannelServos[0].startAngle, - solarPannelServos[0].endAngle)); - auto solarPannelAngleMessage = modelec_interfaces::msg::PCA9685Servo(); - solarPannelAngleMessage.pin = solarPannelServos[0].pin; - solarPannelAngleMessage.angle = solarPannelAngle; - pca9685_publisher_->publish(solarPannelAngleMessage); - last_solar_1_angle = msg->axes[2]; - } - - if (msg->axes[5] != last_solar_2_angle) - { - int solarPannelAngle = static_cast(Modelec::mapValue(static_cast(msg->axes[5]), -1.0f, 1.0f, - solarPannelServos[1].startAngle, - solarPannelServos[1].endAngle)); - auto solarPannelAngleMessage = modelec_interfaces::msg::PCA9685Servo(); - solarPannelAngleMessage.pin = solarPannelServos[1].pin; - solarPannelAngleMessage.angle = solarPannelAngle; - pca9685_publisher_->publish(solarPannelAngleMessage); - last_solar_2_angle = msg->axes[5]; - } - } -} - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} diff --git a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp index 27c5082..ddc4ce0 100644 --- a/src/modelec_gui/include/modelec_gui/pages/map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/map_page.hpp @@ -12,12 +12,12 @@ #include -#include +#include +#include #include #include #include #include -#include #include #include @@ -78,7 +78,8 @@ namespace ModelecGUI { rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr add_waypoint_sub_; + rclcpp::Subscription::SharedPtr add_waypoint_sub_; + rclcpp::Subscription::SharedPtr add_waypoints_sub_; rclcpp::Subscription::SharedPtr odometry_sub_; rclcpp::Subscription::SharedPtr score_sub_; diff --git a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp index 3bbe451..a36b60a 100644 --- a/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/odo_page.hpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include @@ -45,13 +45,13 @@ namespace ModelecGUI rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr sub_; + // rclcpp::Subscription::SharedPtr sub_; // client - rclcpp::Client::SharedPtr client_; - rclcpp::Client::SharedPtr client_start_; - rclcpp::Client::SharedPtr client_get_pid_; - rclcpp::Client::SharedPtr client_set_pid_; + /* rclcpp::Client::SharedPtr client_; + rclcpp::Client::SharedPtr client_start_; + rclcpp::Client::SharedPtr client_get_pid_; + rclcpp::Client::SharedPtr client_set_pid_;*/ void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); }; diff --git a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp index 450ceed..34acacf 100644 --- a/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp +++ b/src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp @@ -12,13 +12,13 @@ #include -#include +#include +#include #include #include #include #include #include -#include #include #include @@ -48,8 +48,6 @@ namespace ModelecGUI void mousePressEvent(QMouseEvent* event) override; - void onOdometryReceived(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); - void OnObstacleReceived(const modelec_interfaces::msg::Obstacle::SharedPtr msg); void resizeEvent(QResizeEvent* event) override; @@ -79,7 +77,8 @@ namespace ModelecGUI rclcpp::Node::SharedPtr node_; - rclcpp::Subscription::SharedPtr add_waypoint_sub_; + rclcpp::Subscription::SharedPtr add_waypoint_sub_; + rclcpp::Subscription::SharedPtr add_waypoints_sub_; rclcpp::Subscription::SharedPtr odometry_sub_; rclcpp::Publisher::SharedPtr go_to_pub_; diff --git a/src/modelec_gui/src/modelec_gui.cpp b/src/modelec_gui/src/modelec_gui.cpp index 8bb770c..278ef02 100644 --- a/src/modelec_gui/src/modelec_gui.cpp +++ b/src/modelec_gui/src/modelec_gui.cpp @@ -13,25 +13,25 @@ namespace ModelecGUI { resize(1200, 800); home_page_ = new HomePage(get_node(), this); - odo_page_ = new OdoPage(get_node(), this); + //odo_page_ = new OdoPage(get_node(), this); test_map_page_ = new TestMapPage(get_node(), this); map_page_ = new MapPage(get_node(), this); - action_page_ = new ActionPage(get_node(), this); - alim_page_ = new AlimPage(get_node(), this); + // action_page_ = new ActionPage(get_node(), this); + // alim_page_ = new AlimPage(get_node(), this); stackedWidget->addWidget(home_page_); - stackedWidget->addWidget(odo_page_); + // stackedWidget->addWidget(odo_page_); stackedWidget->addWidget(test_map_page_); stackedWidget->addWidget(map_page_); - stackedWidget->addWidget(action_page_); - stackedWidget->addWidget(alim_page_); + // stackedWidget->addWidget(action_page_); + // stackedWidget->addWidget(alim_page_); setCentralWidget(stackedWidget); setupMenu(); connect(home_page_, &HomePage::TeamChoose, this, [this]() { - stackedWidget->setCurrentIndex(3); + stackedWidget->setCurrentWidget(map_page_); }); } @@ -68,25 +68,25 @@ namespace ModelecGUI { optionsMenu->addAction(exit_action_); connect(home_action_, &QAction::triggered, this, [this]() { - stackedWidget->setCurrentIndex(0); + stackedWidget->setCurrentWidget(home_page_); home_page_->Init(); map_page_->Reset(); }); connect(odo_action_, &QAction::triggered, this, [this]() { - stackedWidget->setCurrentIndex(1); + stackedWidget->setCurrentWidget(odo_page_); }); connect(action_action_, &QAction::triggered, this, [this]() { - stackedWidget->setCurrentIndex(4); + stackedWidget->setCurrentWidget(action_page_); }); connect(alim_action_, &QAction::triggered, this, [this]() { - stackedWidget->setCurrentIndex(5); + stackedWidget->setCurrentWidget(alim_page_); }); connect(map_action_, &QAction::triggered, this, [this]() { - stackedWidget->setCurrentIndex(2); + stackedWidget->setCurrentWidget(map_page_); }); connect(playmat_map_, &QAction::triggered, this, [this]() { diff --git a/src/modelec_gui/src/pages/home_page.cpp b/src/modelec_gui/src/pages/home_page.cpp index 8b4f67c..736c65e 100644 --- a/src/modelec_gui/src/pages/home_page.cpp +++ b/src/modelec_gui/src/pages/home_page.cpp @@ -2,9 +2,7 @@ #include #include -#include - -#include "../../../modelec_utils/include/modelec_utils/config.hpp" +#include namespace ModelecGUI { @@ -12,12 +10,12 @@ namespace ModelecGUI : QWidget(parent), node_(node), renderer_(new QSvgRenderer(QString(":/img/playmat/2025_FINAL.svg"), this)) { - spawn_pub_ = node_->create_publisher("/strat/spawn", 10); + spawn_pub_ = node_->create_publisher("strat/spawn", 10); auto w = Modelec::Config::get("config.spawn.width_mm"); auto h = Modelec::Config::get("config.spawn.height_mm"); - spawn_sub_ = node_->create_subscription("/nav/spawn", 10, + spawn_sub_ = node_->create_subscription("nav/spawn", 10, [this, w, h](const modelec_interfaces::msg::Spawn::SharedPtr msg) { auto ratioX = 1200 / 3000.0f; @@ -53,9 +51,9 @@ namespace ModelecGUI }); }); - reset_strat_pub_ = node_->create_publisher("/strat/reset", 10); + reset_strat_pub_ = node_->create_publisher("strat/reset", 10); - ask_spawn_client_ = node_->create_client("/nav/ask_spawn"); + ask_spawn_client_ = node_->create_client("nav/ask_spawn"); ask_spawn_client_->wait_for_service(); auto ask_spawn_request_ = std::make_shared(); auto res = ask_spawn_client_->async_send_request(ask_spawn_request_); diff --git a/src/modelec_gui/src/pages/map_page.cpp b/src/modelec_gui/src/pages/map_page.cpp index 9dba091..d3d24ce 100644 --- a/src/modelec_gui/src/pages/map_page.cpp +++ b/src/modelec_gui/src/pages/map_page.cpp @@ -51,9 +51,9 @@ namespace ModelecGUI enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); - add_waypoint_sub_ = node_->create_subscription( + add_waypoint_sub_ = node_->create_subscription( "odometry/add_waypoint", 100, - [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) + [this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg) { if (lastWapointWasEnd) { @@ -74,6 +74,26 @@ namespace ModelecGUI update(); }); + add_waypoints_sub_ = node_->create_subscription( + "odometry/add_waypoints", 10, + [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) + { + qpoints.clear(); + lastWapointWasEnd = false; + + qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_)); + + for (const auto& point : msg->waypoints) + { + qpoints.push_back(QPoint(point.x * ratioBetweenMapAndWidgetX_, + height() - point.y * ratioBetweenMapAndWidgetY_)); + } + + update(); + }); + + odometry_sub_ = node_->create_subscription("odometry/position", 10, [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { @@ -111,7 +131,7 @@ namespace ModelecGUI update(); }); - strat_start_sub_ = node_->create_subscription("/strat/start_time", 10, + strat_start_sub_ = node_->create_subscription("strat/start_time", 10, [this]( const std_msgs::msg::Int64::SharedPtr msg) { @@ -119,7 +139,7 @@ namespace ModelecGUI start_time_ = msg->data; }); - strat_state_sub_ = node_->create_subscription("/strat/state", 10, + strat_state_sub_ = node_->create_subscription("strat/state", 10, [this](const modelec_interfaces::msg::StratState::SharedPtr msg) { if (msg->state == modelec_interfaces::msg::StratState::STOP) diff --git a/src/modelec_gui/src/pages/odo_page.cpp b/src/modelec_gui/src/pages/odo_page.cpp index 48e18b5..bd4eb74 100644 --- a/src/modelec_gui/src/pages/odo_page.cpp +++ b/src/modelec_gui/src/pages/odo_page.cpp @@ -13,7 +13,7 @@ namespace ModelecGUI RCLCPP_INFO(node_->get_logger(), "Start button clicked."); auto request = std::make_shared(); request->start = true; - client_start_->async_send_request( + /*client_start_->async_send_request( request, [this](rclcpp::Client::SharedFuture response) { if (response.get()->success) @@ -24,7 +24,7 @@ namespace ModelecGUI { RCLCPP_ERROR(node_->get_logger(), "Failed to send start command."); } - }); + });*/ }); // Initialize the UI components @@ -49,7 +49,7 @@ namespace ModelecGUI auto request = std::make_shared(); // Make the asynchronous service call - client_get_pid_->async_send_request( + /*client_get_pid_->async_send_request( request, [this](rclcpp::Client::SharedFuture response) { RCLCPP_INFO(node_->get_logger(), "Received PID response."); @@ -68,7 +68,7 @@ namespace ModelecGUI { RCLCPP_ERROR(node_->get_logger(), "Failed to get response for PID request."); } - }); + });*/ }); pPIDBox_ = new QDoubleSpinBox(this); pPIDBox_->setMinimum(0); @@ -106,7 +106,7 @@ namespace ModelecGUI request->d = dPIDBox_->text().toFloat(); // Make the asynchronous service call - client_set_pid_->async_send_request( + /*client_set_pid_->async_send_request( request, [this](rclcpp::Client::SharedFuture response) { if (response.get()->success) @@ -117,7 +117,7 @@ namespace ModelecGUI { RCLCPP_ERROR(node_->get_logger(), "Failed to send set PID command."); } - }); + });*/ }); askSpeed_ = new QPushButton("Ask speed"); @@ -127,7 +127,7 @@ namespace ModelecGUI auto request = std::make_shared(); // Make the asynchronous service call - client_->async_send_request( + /*client_->async_send_request( request, [this](rclcpp::Client::SharedFuture response) { if (auto res = response.get()) @@ -143,7 +143,7 @@ namespace ModelecGUI { RCLCPP_ERROR(node_->get_logger(), "Failed to get response for speed request."); } - }); + });*/ }); xSpeedBox_ = new QLineEdit("x speed: ?"); @@ -169,11 +169,11 @@ namespace ModelecGUI setLayout(mainLayout_); // Set up subscription - sub_ = node_->create_subscription( - "/odometry/position", 10, - std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1)); + /*sub_ = node_->create_subscription( + "/odom/position", 10, + std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));*/ - client_ = node_->create_client("odometry/speed"); + /*client_ = node_->create_client("odometry/speed"); while (!client_->wait_for_service(std::chrono::seconds(1))) { if (!rclcpp::ok()) @@ -216,7 +216,7 @@ namespace ModelecGUI return; } RCLCPP_INFO(node_->get_logger(), "Service not available, waiting again..."); - } + }*/ } OdoPage::~OdoPage() = default; diff --git a/src/modelec_gui/src/pages/test_map_page.cpp b/src/modelec_gui/src/pages/test_map_page.cpp index 85638d0..821b3b5 100644 --- a/src/modelec_gui/src/pages/test_map_page.cpp +++ b/src/modelec_gui/src/pages/test_map_page.cpp @@ -25,17 +25,17 @@ namespace ModelecGUI enemy_length_ = Modelec::Config::get("config.enemy.size.length_mm", 300); enemy_width_ = Modelec::Config::get("config.enemy.size.width_mm", 300); - add_waypoint_sub_ = node_->create_subscription( + add_waypoint_sub_ = node_->create_subscription( "odometry/add_waypoint", 100, - [this](const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) + [this](const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg) { if (lastWapointWasEnd) { qpoints.clear(); lastWapointWasEnd = false; - qpoints.push_back(QPoint(robotPos.x * ratioBetweenMapAndWidgetX_, - height() - robotPos.y * ratioBetweenMapAndWidgetY_)); + qpoints.emplace_back(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_); } if (msg->is_end) @@ -43,8 +43,27 @@ namespace ModelecGUI lastWapointWasEnd = true; } - qpoints.push_back(QPoint(msg->x * ratioBetweenMapAndWidgetX_, - height() - msg->y * ratioBetweenMapAndWidgetY_)); + qpoints.emplace_back(msg->x * ratioBetweenMapAndWidgetX_, + height() - msg->y * ratioBetweenMapAndWidgetY_); + update(); + }); + + add_waypoints_sub_ = node_->create_subscription( + "odometry/add_waypoints", 10, + [this](const modelec_interfaces::msg::OdometryWaypoints::SharedPtr msg) + { + qpoints.clear(); + lastWapointWasEnd = false; + + qpoints.emplace_back(robotPos.x * ratioBetweenMapAndWidgetX_, + height() - robotPos.y * ratioBetweenMapAndWidgetY_); + + for (const auto& point : msg->waypoints) + { + qpoints.emplace_back(point.x * ratioBetweenMapAndWidgetX_, + height() - point.y * ratioBetweenMapAndWidgetY_); + } + update(); }); @@ -229,7 +248,8 @@ namespace ModelecGUI msg.close = true; if (show_obstacle_) { - msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL; + msg.mask = modelec_interfaces::msg::OdometryGoTo::FREE | modelec_interfaces::msg::OdometryGoTo::WALL | + modelec_interfaces::msg::OdometryGoTo::OBSTACLE | modelec_interfaces::msg::OdometryGoTo::ENEMY; } else { diff --git a/src/modelec_interfaces/CMakeLists.txt b/src/modelec_interfaces/CMakeLists.txt index 662ce81..cf23ddb 100644 --- a/src/modelec_interfaces/CMakeLists.txt +++ b/src/modelec_interfaces/CMakeLists.txt @@ -12,47 +12,44 @@ find_package(geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} - "msg/Alim/AlimEmg.msg" - "msg/Odometry/OdometryPos.msg" - "msg/Odometry/OdometryGoTo.msg" - "msg/Odometry/OdometrySpeed.msg" - "msg/Odometry/OdometryToF.msg" - "msg/Odometry/OdometryWaypointReach.msg" - "msg/Odometry/OdometryAddWaypoint.msg" - "msg/Odometry/OdometryStart.msg" - "msg/Odometry/PID/OdometryPid.msg" - "msg/Strat/StratState.msg" - "msg/Map/Map.msg" - "msg/Map/Obstacle.msg" - "msg/Map/Spawn.msg" - "msg/PCA9685Servo.msg" - "msg/ServoMode.msg" - "msg/Solenoid.msg" - "msg/Button.msg" - "msg/Action/ActionAscPos.msg" - "msg/Action/ActionRelayState.msg" - "msg/Action/ActionServoPos.msg" - "msg/Action/ActionExec.msg" - "srv/Alim/AlimOut.srv" - "srv/Alim/AlimIn.srv" - "srv/Alim/AlimTemp.srv" - "srv/Alim/AlimBau.srv" - "srv/Alim/AlimEmg.srv" - "srv/Odometry/OdometryPosition.srv" - "srv/Odometry/OdometrySpeed.srv" - "srv/Odometry/OdometryToF.srv" - "srv/Odometry/OdometryStart.srv" - "srv/Odometry/OdometryAddWaypoint.srv" - "srv/Odometry/PID/OdometryGetPid.srv" - "srv/Odometry/PID/OdometrySetPid.srv" - "srv/Map/Map.srv" - "srv/Map/MapSize.srv" - "srv/AddServoMotor.srv" - "srv/AddSolenoid.srv" - "srv/Tirette.srv" - "srv/AddButton.srv" - "srv/Button.srv" - "srv/AddSerialListener.srv" + "msg/Alim/AlimEmg.msg" + "msg/Odometry/OdometryPos.msg" + "msg/Odometry/OdometryGoTo.msg" + "msg/Odometry/OdometrySpeed.msg" + "msg/Odometry/OdometryToF.msg" + "msg/Odometry/OdometryRealign.msg" + "msg/Odometry/OdometryWaypoint.msg" + "msg/Odometry/OdometryWaypoints.msg" + "msg/Odometry/OdometryStart.msg" + "msg/Odometry/PID/OdometryPid.msg" + "msg/Strat/StratState.msg" + "msg/Map/Map.msg" + "msg/Map/Obstacle.msg" + "msg/Map/Spawn.msg" + "msg/Action/ActionAscPos.msg" + "msg/Action/ActionRelayState.msg" + "msg/Action/ActionServoPos.msg" + "msg/Action/ActionExec.msg" + "srv/Alim/AlimOut.srv" + "srv/Alim/AlimIn.srv" + "srv/Alim/AlimTemp.srv" + "srv/Alim/AlimBau.srv" + "srv/Alim/AlimEmg.srv" + "srv/Odometry/OdometryPosition.srv" + "srv/Odometry/OdometrySpeed.srv" + "srv/Odometry/OdometryToF.srv" + "srv/Odometry/OdometryStart.srv" + "srv/Odometry/OdometryAddWaypoint.srv" + "srv/Odometry/PID/OdometryGetPid.srv" + "srv/Odometry/PID/OdometrySetPid.srv" + "srv/Map/Map.srv" + "srv/Map/MapSize.srv" + "srv/AddServoMotor.srv" + "srv/AddSolenoid.srv" + "srv/Tirette.srv" + "srv/AddButton.srv" + "srv/Button.srv" + "srv/AddSerialListener.srv" DEPENDENCIES std_msgs geometry_msgs ) diff --git a/src/modelec_interfaces/msg/Alim/AlimEmg.msg b/src/modelec_interfaces/msg/Alim/AlimEmg.msg index 402f094..9ce4b5a 100644 --- a/src/modelec_interfaces/msg/Alim/AlimEmg.msg +++ b/src/modelec_interfaces/msg/Alim/AlimEmg.msg @@ -1 +1 @@ -bool activate +bool activate \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Odometry/OdometryRealign.msg b/src/modelec_interfaces/msg/Odometry/OdometryRealign.msg new file mode 100644 index 0000000..042339b --- /dev/null +++ b/src/modelec_interfaces/msg/Odometry/OdometryRealign.msg @@ -0,0 +1,13 @@ +int8 BACKWARD=1 +int8 FORWARD=2 + +int8 X=3 +int8 Y=4 +int8 XY=5 + +int8 direction +int8 axis +float32 x +float32 y +float32 theta +bool force 0 \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg b/src/modelec_interfaces/msg/Odometry/OdometryWaypoint.msg similarity index 100% rename from src/modelec_interfaces/msg/Odometry/OdometryAddWaypoint.msg rename to src/modelec_interfaces/msg/Odometry/OdometryWaypoint.msg diff --git a/src/modelec_interfaces/msg/Odometry/OdometryWaypointReach.msg b/src/modelec_interfaces/msg/Odometry/OdometryWaypointReach.msg deleted file mode 100644 index 98d97a8..0000000 --- a/src/modelec_interfaces/msg/Odometry/OdometryWaypointReach.msg +++ /dev/null @@ -1 +0,0 @@ -uint8 id \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Odometry/OdometryWaypoints.msg b/src/modelec_interfaces/msg/Odometry/OdometryWaypoints.msg new file mode 100644 index 0000000..cd15e5c --- /dev/null +++ b/src/modelec_interfaces/msg/Odometry/OdometryWaypoints.msg @@ -0,0 +1 @@ +OdometryWaypoint[] waypoints \ No newline at end of file diff --git a/src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg b/src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg index e5307b7..dda65e0 100644 --- a/src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg +++ b/src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg @@ -1,3 +1,9 @@ +string POS="POS" +string THETA="THETA" +string LEFT="LEFT" +string RIGHT="RIGHT" + +string name float32 p float32 i float32 d \ No newline at end of file diff --git a/src/modelec_interfaces/msg/PCA9685Servo.msg b/src/modelec_interfaces/msg/PCA9685Servo.msg deleted file mode 100644 index 5e93de7..0000000 --- a/src/modelec_interfaces/msg/PCA9685Servo.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 pin -int64 angle \ No newline at end of file diff --git a/src/modelec_interfaces/msg/ServoMode.msg b/src/modelec_interfaces/msg/ServoMode.msg deleted file mode 100644 index 85678d2..0000000 --- a/src/modelec_interfaces/msg/ServoMode.msg +++ /dev/null @@ -1,11 +0,0 @@ -uint8 PINCE_CLOSED=0 -uint8 PINCE_MIDDLE=1 -uint8 PINCE_OPEN=2 -uint8 PINCE_FULLY_OPEN=3 -uint8 ARM_BOTTOM=4 -uint8 ARM_MIDDLE=5 -uint8 ARM_TOP=6 - -uint8 pin -uint8 mode -bool is_arm diff --git a/src/modelec_interfaces/msg/Solenoid.msg b/src/modelec_interfaces/msg/Solenoid.msg deleted file mode 100644 index 364946d..0000000 --- a/src/modelec_interfaces/msg/Solenoid.msg +++ /dev/null @@ -1,2 +0,0 @@ -uint8 pin -bool state \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp index 1df0646..c2b71fc 100644 --- a/src/modelec_strat/include/modelec_strat/navigation_helper.hpp +++ b/src/modelec_strat/include/modelec_strat/navigation_helper.hpp @@ -1,8 +1,8 @@ #pragma once #include -#include -#include +#include +#include #include #include #include @@ -41,6 +41,13 @@ namespace Modelec // void SendWaypoint() const; // void SendWaypoint(const std::vector& waypoints) const; + void SendWaypoint() const; + void SendWaypoint(const std::vector& waypoints) const; + + void SendWaypoints() const; + void SendWaypoints(const std::vector& waypoints) const; + void SendWaypoints(const WaypointsMsg& waypoints) const; + void SendGoTo(); void AddWaypoint(const PosMsg& pos, int index); @@ -86,7 +93,6 @@ namespace Modelec PosMsg::SharedPtr GetHomePosition(); void OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); - void OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); void OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg); @@ -104,7 +110,7 @@ namespace Modelec Point GetSpawn() const; protected: - void OnWaypointReach(const WaypointReachMsg::SharedPtr msg); + void OnWaypointReach(const WaypointMsg::SharedPtr msg); void OnPos(const PosMsg::SharedPtr msg); @@ -140,15 +146,15 @@ namespace Modelec std::map> deposite_zones_; - rclcpp::Subscription::SharedPtr waypoint_reach_sub_; + rclcpp::Subscription::SharedPtr waypoint_reach_sub_; rclcpp::Publisher::SharedPtr waypoint_pub_; + rclcpp::Publisher::SharedPtr waypoints_pub_; rclcpp::Subscription::SharedPtr go_to_sub_; rclcpp::Subscription::SharedPtr pos_sub_; rclcpp::Publisher::SharedPtr pos_pub_; rclcpp::Subscription::SharedPtr enemy_pos_sub_; - rclcpp::Subscription::SharedPtr close_enemy_pos_sub_; rclcpp::Subscription::SharedPtr enemy_pos_long_time_sub_; rclcpp::Publisher::SharedPtr start_odo_pub_; @@ -165,4 +171,4 @@ namespace Modelec rclcpp::Time last_odo_get_pos_time_; }; -} +} \ No newline at end of file diff --git a/src/modelec_strat/include/modelec_strat/pathfinding.hpp b/src/modelec_strat/include/modelec_strat/pathfinding.hpp index e350775..1c31468 100644 --- a/src/modelec_strat/include/modelec_strat/pathfinding.hpp +++ b/src/modelec_strat/include/modelec_strat/pathfinding.hpp @@ -6,8 +6,8 @@ #include -#include -#include +#include +#include #include #include #include @@ -21,8 +21,8 @@ namespace Modelec { - using WaypointMsg = modelec_interfaces::msg::OdometryAddWaypoint; - using WaypointReachMsg = modelec_interfaces::msg::OdometryWaypointReach; + using WaypointMsg = modelec_interfaces::msg::OdometryWaypoint; + using WaypointsMsg = modelec_interfaces::msg::OdometryWaypoints; using PosMsg = modelec_interfaces::msg::OdometryPos; using WaypointListMsg = std::vector; @@ -65,7 +65,7 @@ namespace Modelec Pathfinding(const rclcpp::Node::SharedPtr& node); - rclcpp::Node::SharedPtr GetNode() const; + [[nodiscard]] rclcpp::Node::SharedPtr GetNode() const; std::pair FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal, bool isClose = false, @@ -77,7 +77,7 @@ namespace Modelec void SetCurrentPos(const PosMsg::SharedPtr& pos); - std::shared_ptr GetObstacle(int id) const; + [[nodiscard]] std::shared_ptr GetObstacle(int id) const; void RemoveObstacle(int id); @@ -162,4 +162,4 @@ namespace Modelec return closest_obstacle; } -} +} \ No newline at end of file diff --git a/src/modelec_strat/src/navigation_helper.cpp b/src/modelec_strat/src/navigation_helper.cpp index 4974837..5b8d287 100644 --- a/src/modelec_strat/src/navigation_helper.cpp +++ b/src/modelec_strat/src/navigation_helper.cpp @@ -1,14 +1,12 @@ #include #include #include - -#include "../../modelec_utils/include/modelec_utils/config.hpp" +#include namespace Modelec { NavigationHelper::NavigationHelper() - { - } + = default; NavigationHelper::NavigationHelper(const rclcpp::Node::SharedPtr& node) : node_(node) { @@ -20,14 +18,15 @@ namespace Modelec SetupSpawn(); - waypoint_reach_sub_ = node_->create_subscription( + waypoint_reach_sub_ = node_->create_subscription( "odometry/waypoint_reach", 10, - [this](const WaypointReachMsg::SharedPtr msg) + [this](const WaypointMsg::SharedPtr msg) { OnWaypointReach(msg); }); - waypoint_pub_ = node_->create_publisher("odometry/add_waypoint", 100); + waypoint_pub_ = node_->create_publisher("odometry/add_waypoint", 30); + waypoints_pub_ = node_->create_publisher("odometry/add_waypoints", 30); pos_sub_ = node_->create_subscription( "odometry/position", 20, @@ -51,21 +50,14 @@ namespace Modelec OnEnemyPosition(msg); }); - close_enemy_pos_sub_ = node_->create_subscription( - "enemy/position/emergency", 10, - [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) - { - OnEnemyPositionClose(msg); - }); - enemy_pos_long_time_sub_ = node_->create_subscription( - "/enemy/long_time", 10, + "enemy/long_time", 10, [this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { OnEnemyPositionLongTime(msg); }); - start_odo_pub_ = node_->create_publisher("/odometry/start", 10); + start_odo_pub_ = node_->create_publisher("odometry/start", 10); std::string deposite_zone_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/deposite_zone.xml"; @@ -77,7 +69,7 @@ namespace Modelec spawn_pub_ = node_->create_publisher("nav/spawn", 10); ask_spawn_srv_ = node->create_service( - "/nav/ask_spawn", [this](const std_srvs::srv::Empty::Request::SharedPtr, + "nav/ask_spawn", [this](const std_srvs::srv::Empty::Request::SharedPtr, const std_srvs::srv::Empty::Response::SharedPtr) { for (auto& ys : spawn_yellow_) @@ -137,13 +129,13 @@ namespace Modelec { last_odo_get_pos_time_ = node_->now(); std_msgs::msg::Empty empty_msg; - odo_get_pos_pub_->publish(empty_msg); + // odo_get_pos_pub_->publish(empty_msg); } } void NavigationHelper::SendGoTo() { - while (!waypoint_queue_.empty()) + /*while (!waypoint_queue_.empty()) { waypoint_queue_.pop(); } @@ -156,28 +148,44 @@ namespace Modelec } auto w = waypoint_queue_.front().ToMsg(); - /*RCLCPP_INFO(node_->get_logger(), "Sending waypoint: x: %d, y: %d, theta: %f, id: %d", - w.x, w.y, w.theta, w.id);*/ waypoint_pub_->publish(w); - waypoint_queue_.pop(); + waypoint_queue_.pop();*/ + + SendWaypoints(); } - /* void NavigationHelper::SendWaypoint() const + void NavigationHelper::SendWaypoint() const + { + for (auto& w : waypoints_) { - for (auto& w : waypoints_) - { - waypoint_pub_->publish(w.ToMsg()); - } + waypoint_pub_->publish(w.ToMsg()); + } + } + + void NavigationHelper::SendWaypoint(const std::vector& waypoints) const + { + for (auto& w : waypoints) + { + waypoint_pub_->publish(w); + } + } + + void NavigationHelper::SendWaypoints() const + { + WaypointsMsg waypoints_msg; + + for (auto& w : waypoints_) + { + waypoints_msg.waypoints.push_back(w.ToMsg()); } - void NavigationHelper::SendWaypoint(const std::vector& waypoints) const - { - for (auto& w : waypoints) - { - waypoint_pub_->publish(w); - } - } - */ + SendWaypoints(waypoints_msg); + } + + void NavigationHelper::SendWaypoints(const WaypointsMsg& waypoints) const + { + waypoints_pub_->publish(waypoints); + } void NavigationHelper::AddWaypoint(const PosMsg& pos, int index) { @@ -302,7 +310,7 @@ namespace Modelec { double angle = std::atan2(pos.y - current_pos_->y, pos.x - current_pos_->x); - if (std::abs(angle - current_pos_->theta) > M_PI / 3) + if (std::abs(angle - current_pos_->theta) > M_PI / 4) { Rotate(angle); return true; @@ -518,7 +526,8 @@ namespace Modelec auto zonePoint = zone->GetNextPotPos().GetTakeBasePosition(); double distance = Point::distance(posPoint, zonePoint); double enemy_distance = Point::distance(enemyPos, zone->GetPosition()); - double s = distance + enemy_distance * factor_close_enemy_; + double theta = std::abs(Point::angleDiff(posPoint, zonePoint)); + double s = distance + enemy_distance * factor_close_enemy_ + theta * 2; if (s < score) { score = s; @@ -599,31 +608,6 @@ namespace Modelec } } - void NavigationHelper::OnEnemyPositionClose(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) - { - if (!last_was_close_enemy_) - { - RCLCPP_INFO(node_->get_logger(), "Enemy is close, replanning..."); - - last_was_close_enemy_ = true; - - pathfinding_->OnEnemyPosition(msg); - last_enemy_pos_ = *msg; - - std_msgs::msg::Bool start_odo_msg; - start_odo_msg.data = false; - start_odo_pub_->publish(start_odo_msg); - - /*waypoints_.clear(); - - Waypoint w(*msg, -1, false); - - waypoints_.emplace_back(w); - - SendGoTo();*/ - } - } - void NavigationHelper::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { pathfinding_->OnEnemyPositionLongTime(msg); @@ -632,8 +616,7 @@ namespace Modelec for (auto& [id, zone] : deposite_zones_) { auto zonePos = zone->GetPosition(); - if (Point::distance(enemy_pos, zonePos) < pathfinding_->robot_width_mm_ + (zone->GetWidth() / 2) + - pathfinding_->enemy_margin_mm_) + if (Point::distance(enemy_pos, zonePos) < (zone->GetWidth() / 2) + pathfinding_->enemy_margin_mm_) { std::shared_ptr obs = std::make_shared( id, zonePos.x, zonePos.y, zonePos.theta, zone->GetWidth(), zone->GetHeight(), "enemy-zone"); @@ -694,13 +677,13 @@ namespace Modelec { if (last_go_to_.goal) { - if (GoTo(last_go_to_.goal, last_go_to_.isClose, last_go_to_.collisionMask) != Pathfinding::FREE) + if (GoToRotateFirst(last_go_to_.goal, last_go_to_.isClose, last_go_to_.collisionMask) != Pathfinding::FREE) { - if (GoTo(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE) + if (GoToRotateFirst(last_go_to_.goal, true, last_go_to_.collisionMask) != Pathfinding::FREE) { if (!force) return false; - if (GoTo(current_pos_, true, + if (GoToRotateFirst(current_pos_, true, Pathfinding::FREE | Pathfinding::WALL | Pathfinding::OBSTACLE | Pathfinding::ENEMY) != Pathfinding::FREE) { @@ -742,9 +725,9 @@ namespace Modelec return spawn_; } - void NavigationHelper::OnWaypointReach(const WaypointReachMsg::SharedPtr) + void NavigationHelper::OnWaypointReach(const WaypointMsg::SharedPtr msg) { - /*for (auto& waypoint : waypoints_) + for (auto& waypoint : waypoints_) { if (waypoint.id == msg->id) { @@ -759,12 +742,12 @@ namespace Modelec { waypoints_.emplace_back(w); } - SendWaypoint(); + SendGoTo(); } } - }*/ + } - if (await_rotate_) + /*if (await_rotate_) { await_rotate_ = false; @@ -789,7 +772,7 @@ namespace Modelec { waypoints_.back().reached = true; } - } + }*/ } void NavigationHelper::OnPos(const PosMsg::SharedPtr msg) @@ -836,4 +819,4 @@ namespace Modelec Config::get("config.spawn.blue.bottom@theta") ); } -} +} \ No newline at end of file diff --git a/src/modelec_strat/src/pathfinding.cpp b/src/modelec_strat/src/pathfinding.cpp index b37625b..91ee887 100644 --- a/src/modelec_strat/src/pathfinding.cpp +++ b/src/modelec_strat/src/pathfinding.cpp @@ -3,33 +3,27 @@ #include #include -namespace Modelec -{ - struct AStarNode - { +namespace Modelec { + struct AStarNode { int x, y; double g = std::numeric_limits::infinity(); // Cost from start double f = std::numeric_limits::infinity(); // g + heuristic int parent_x = -1; int parent_y = -1; - bool operator>(const AStarNode& other) const - { + bool operator>(const AStarNode &other) const { return f > other.f; } }; - double Heuristic(int x1, int y1, int x2, int y2) - { + double Heuristic(int x1, int y1, int x2, int y2) { return std::hypot(x1 - x2, y1 - y2); // Euclidean distance } - Pathfinding::Pathfinding() - { + Pathfinding::Pathfinding() { } - Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr& node) : node_(node) - { + Pathfinding::Pathfinding(const rclcpp::Node::SharedPtr &node) : node_(node) { map_width_mm_ = Config::get("config.map.size.map_width_mm", 3000); map_height_mm_ = Config::get("config.map.size.map_height_mm", 2000); @@ -48,16 +42,14 @@ namespace Modelec grid_height_ = Config::get("config.map.size.grid_height", 200); std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") + - "/data/obstacles.xml"; - if (!LoadObstaclesFromXML(obstacles_path)) - { + "/data/obstacles.xml"; + if (!LoadObstaclesFromXML(obstacles_path)) { RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacles from XML"); } obstacle_add_sub_ = node_->create_subscription( "obstacle/add", 10, - [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) - { + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { RCLCPP_INFO(node_->get_logger(), "Obstacle add request received"); AddObstacle(std::make_shared(*msg)); }); @@ -67,8 +59,7 @@ namespace Modelec obstacle_remove_sub_ = node_->create_subscription( "obstacle/remove", 10, - [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) - { + [this](const modelec_interfaces::msg::Obstacle::SharedPtr msg) { RCLCPP_INFO(node_->get_logger(), "Obstacle remove request received"); RemoveObstacle(msg->id); }); @@ -79,24 +70,21 @@ namespace Modelec ask_obstacle_srv_ = node_->create_service( "nav/ask_map_obstacle", [this](const std::shared_ptr request, - const std::shared_ptr response) - { + const std::shared_ptr response) { HandleAskObstacleRequest(request, response); }); map_srv_ = node_->create_service( "nav/map", [this](const std::shared_ptr request, - const std::shared_ptr response) - { + const std::shared_ptr response) { HandleMapRequest(request, response); }); map_size_srv_ = node_->create_service( "nav/map_size", [this](const std::shared_ptr request, - const std::shared_ptr response) - { + const std::shared_ptr response) { HandleMapSizeRequest(request, response); }); @@ -104,8 +92,7 @@ namespace Modelec "odometry/add_waypoint", 100); } - rclcpp::Node::SharedPtr Pathfinding::GetNode() const - { + rclcpp::Node::SharedPtr Pathfinding::GetNode() const { return node_; } @@ -138,11 +125,9 @@ namespace Modelec } }*/ - std::pair Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal, - bool isClose, int collisionMask) - { - if (!start || !goal) - { + std::pair Pathfinding::FindPath(const PosMsg::SharedPtr &start, const PosMsg::SharedPtr &goal, + bool isClose, int collisionMask) { + if (!start || !goal) { RCLCPP_WARN(node_->get_logger(), "Start or Goal position is null"); return {-3, WaypointListMsg()}; } @@ -156,13 +141,10 @@ namespace Modelec int inflate_x; int inflate_y; - if (isClose) - { + if (isClose) { inflate_x = std::ceil(((robot_width_mm_) / 2.0f) / cell_size_mm_x); inflate_y = std::ceil(((robot_length_mm_) / 2.0f) / cell_size_mm_y); - } - else - { + } else { inflate_x = std::ceil(((robot_width_mm_ + margin_mm_) / 2.0f) / cell_size_mm_x); inflate_y = std::ceil(((robot_length_mm_ + margin_mm_) / 2.0f) / cell_size_mm_y); } @@ -170,27 +152,22 @@ namespace Modelec // 1. Build fresh empty grid grid_.clear(); grid_.resize(grid_height_); - for (auto& row : grid_) - { + for (auto &row: grid_) { row.assign(grid_width_, FREE); } - if (has_enemy_pos_) - { + if (has_enemy_pos_) { int ex = (last_enemy_pos_.x / cell_size_mm_x); int ey = ((map_height_mm_ - last_enemy_pos_.y) / cell_size_mm_y); const int inflate_enemy_x = std::ceil((enemy_margin_mm_ + (enemy_width_mm_ / 2)) / cell_size_mm_x) + - inflate_x; + inflate_x; const int inflate_enemy_y = std::ceil((enemy_margin_mm_ + (enemy_length_mm_ / 2)) / cell_size_mm_y) + - inflate_y; + inflate_y; - for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y) - { - for (int x = ex - inflate_enemy_x; x <= ex + inflate_enemy_x; ++x) - { - if (x >= 0 && y >= 0 && x < grid_width_ && y < grid_height_) - { + for (int y = ey - inflate_enemy_y; y <= ey + inflate_enemy_y; ++y) { + for (int x = ex - inflate_enemy_x; x <= ex + inflate_enemy_x; ++x) { + if (x >= 0 && y >= 0 && x < grid_width_ && y < grid_height_) { grid_[y][x] |= ENEMY; } } @@ -198,20 +175,16 @@ namespace Modelec } // Bord gauche et droit - for (int y = 0; y < grid_height_; ++y) - { - for (int x = 0; x < inflate_x; ++x) - { + for (int y = 0; y < grid_height_; ++y) { + for (int x = 0; x < inflate_x; ++x) { grid_[y][x] |= WALL; grid_[y][grid_width_ - 1 - x] |= WALL; } } // Bord haut et bas - for (int x = 0; x < grid_width_; ++x) - { - for (int y = 0; y < inflate_y; ++y) - { + for (int x = 0; x < grid_width_; ++x) { + for (int y = 0; y < inflate_y; ++y) { grid_[y][x] |= WALL; grid_[grid_height_ - 1 - y][x] |= WALL; } @@ -219,8 +192,7 @@ namespace Modelec // 2. Fill obstacles with inflation // TODO some bug exist with the inflate - for (const auto& [id, obstacle] : obstacle_map_) - { + for (const auto &[id, obstacle]: obstacle_map_) { float cx = obstacle->GetX(); float cy = obstacle->GetY(); float width = obstacle->GetWidth() + inflate_x * 2 * cell_size_mm_x; @@ -231,12 +203,11 @@ namespace Modelec float dy = height / 2.0f; // Compute corners in local space and rotate+translate to world - std::vector> corners = { + std::vector > corners = { {-dx, -dy}, {dx, -dy}, {dx, dy}, {-dx, dy} }; - for (auto& [x, y] : corners) - { + for (auto &[x, y]: corners) { float rx = x * std::cos(theta) - y * std::sin(theta); float ry = x * std::sin(theta) + y * std::cos(theta); x = rx + cx; @@ -249,24 +220,21 @@ namespace Modelec float min_y = corners[0].second; float max_y = corners[0].second; - for (const auto& [x, y] : corners) - { + for (const auto &[x, y]: corners) { min_x = std::min(min_x, x); max_x = std::max(max_x, x); min_y = std::min(min_y, y); max_y = std::max(max_y, y); } - int x_start = std::max(0, (int)(min_x / cell_size_mm_x)); - int x_end = std::min(grid_width_ - 1, (int)(max_x / cell_size_mm_x)); - int y_start = std::max(0, (int)(min_y / cell_size_mm_y)); - int y_end = std::min(grid_height_ - 1, (int)(max_y / cell_size_mm_y)); + int x_start = std::max(0, (int) (min_x / cell_size_mm_x)); + int x_end = std::min(grid_width_ - 1, (int) (max_x / cell_size_mm_x)); + int y_start = std::max(0, (int) (min_y / cell_size_mm_y)); + int y_end = std::min(grid_height_ - 1, (int) (max_y / cell_size_mm_y)); // Mark cells that fall inside rotated rectangle - for (int y = y_start; y <= y_end; ++y) - { - for (int x = x_start; x <= x_end; ++x) - { + for (int y = y_start; y <= y_end; ++y) { + for (int x = x_start; x <= x_end; ++x) { // Convert cell center to world space float wx = (x + 0.5f) * cell_size_mm_x; float wy = (y + 0.5f) * cell_size_mm_y; @@ -278,11 +246,9 @@ namespace Modelec float lx = dx_ * std::cos(-theta) - dy_ * std::sin(-theta); float ly = dx_ * std::sin(-theta) + dy_ * std::cos(-theta); - if (std::abs(lx) <= dx + 1 && std::abs(ly) <= dy + 1) - { + if (std::abs(lx) <= dx + 1 && std::abs(ly) <= dy + 1) { int gy = (grid_height_ - 1) - y; - if (x >= 0 && gy >= 0 && x < grid_width_ && gy < grid_height_) - { + if (x >= 0 && gy >= 0 && x < grid_width_ && gy < grid_height_) { grid_[gy][x] |= OBSTACLE; } } @@ -300,38 +266,31 @@ namespace Modelec if (start_x < 0 || start_y < 0 || goal_x < 0 || goal_y < 0 || start_x >= grid_width_ || start_y >= grid_height_ || - goal_x >= grid_width_ || goal_y >= grid_height_) - { + goal_x >= grid_width_ || goal_y >= grid_height_) { RCLCPP_WARN(node_->get_logger(), "Start or Goal out of bounds"); return {-2, waypoints}; } - if (!TestCollision(start_x, start_y, collisionMask) || !TestCollision(goal_x, goal_y, collisionMask)) - { - if (!TestCollision(start_x, start_y, collisionMask)) - { + if (!TestCollision(start_x, start_y, collisionMask) || !TestCollision(goal_x, goal_y, collisionMask)) { + if (!TestCollision(start_x, start_y, collisionMask)) { RCLCPP_WARN(node_->get_logger(), "Start inside an obstacle"); return {grid_[start_y][start_x], waypoints}; - } - else - { + } else { RCLCPP_WARN(node_->get_logger(), "Goal inside an obstacle"); return {grid_[goal_y][goal_x], waypoints}; } } // 4. A* algorithm - std::priority_queue, std::greater> open_list; + std::priority_queue, std::greater > open_list; std::unordered_map all_nodes; - auto hash = [](int x, int y) - { - return (int64_t)x << 32 | (uint32_t)y; + auto hash = [](int x, int y) { + return (int64_t) x << 32 | (uint32_t) y; }; - auto neighbors = [](int x, int y) - { - return std::vector>{ + auto neighbors = [](int x, int y) { + return std::vector >{ {x + 1, y}, {x - 1, y}, {x, y + 1}, {x, y - 1}, {x + 1, y + 1}, {x - 1, y + 1}, {x + 1, y - 1}, {x - 1, y - 1} // diagonals }; @@ -346,20 +305,17 @@ namespace Modelec bool path_found = false; - while (!open_list.empty()) - { + while (!open_list.empty()) { AStarNode current = open_list.top(); open_list.pop(); - if (current.x == goal_x && current.y == goal_y) - { + if (current.x == goal_x && current.y == goal_y) { path_found = true; break; } - for (const auto& [nx, ny] : neighbors(current.x, current.y)) - { - if (nx < 0 || ny < 0 || ny >= (int)grid_.size() || nx >= (int)grid_[0].size()) + for (const auto &[nx, ny]: neighbors(current.x, current.y)) { + if (nx < 0 || ny < 0 || ny >= (int) grid_.size() || nx >= (int) grid_[0].size()) continue; if (!TestCollision(nx, ny, collisionMask)) @@ -368,8 +324,7 @@ namespace Modelec double tentative_g = current.g + Heuristic(current.x, current.y, nx, ny); int64_t neighbor_hash = hash(nx, ny); - if (all_nodes.find(neighbor_hash) == all_nodes.end() || tentative_g < all_nodes[neighbor_hash].g) - { + if (all_nodes.find(neighbor_hash) == all_nodes.end() || tentative_g < all_nodes[neighbor_hash].g) { AStarNode neighbor; neighbor.x = nx; neighbor.y = ny; @@ -383,19 +338,17 @@ namespace Modelec } } - if (!path_found) - { + if (!path_found) { RCLCPP_WARN(node_->get_logger(), "No path found"); return {-1, waypoints}; } // 5. Reconstruct path - std::vector> path; + std::vector > path; int cx = goal_x; int cy = goal_y; - while (!(cx == start_x && cy == start_y)) - { + while (!(cx == start_x && cy == start_y)) { path.emplace_back(cx, cy); auto it = all_nodes.find(hash(cx, cy)); if (it == all_nodes.end()) @@ -407,13 +360,11 @@ namespace Modelec std::reverse(path.begin(), path.end()); // 6. Path smoothing - std::vector> smooth_path; + std::vector > smooth_path; size_t current = 0; - while (current < path.size()) - { + while (current < path.size()) { size_t next = current + 1; - while (next < path.size()) - { + while (next < path.size()) { bool clear = true; int x0 = path[current].first; int y0 = path[current].second; @@ -428,23 +379,19 @@ namespace Modelec int x = x0; int y = y0; - while (true) - { - if (!TestCollision(x, y, collisionMask)) - { + while (true) { + if (!TestCollision(x, y, collisionMask)) { clear = false; break; } if (x == x1 && y == y1) break; int e2 = 2 * err; - if (e2 >= dy) - { + if (e2 >= dy) { err += dy; x += sx; } - if (e2 <= dx) - { + if (e2 <= dx) { err += dx; y += sy; } @@ -456,8 +403,7 @@ namespace Modelec } smooth_path.push_back(path[current]); - if (next == path.size()) - { + if (next == path.size()) { smooth_path.push_back(path.back()); break; } @@ -498,13 +444,11 @@ namespace Modelec // 7. Fill Waypoints (reconvertir grille -> millimètres) int id = 0; - for (size_t i = 0; i < smooth_path.size(); ++i) - { - const auto& [x, y] = smooth_path[i]; + for (size_t i = 0; i < smooth_path.size(); ++i) { + const auto &[x, y] = smooth_path[i]; // Skip first point if it's too close to robot position - if (i == 0) - { + if (i == 0) { float world_x = x * cell_size_mm_x; float world_y = (grid_height_ - 1 - y) * cell_size_mm_y; @@ -520,16 +464,15 @@ namespace Modelec wp.y = (grid_height_ - 1 - y) * cell_size_mm_y; // Calculer l'angle entre le point actuel et le prochain point - if (i < smooth_path.size() - 1) // Si ce n'est pas le dernier point - { - const auto& [next_x, next_y] = smooth_path[i + 1]; + if (i < smooth_path.size() - 1) { + const auto &[next_x, next_y] = smooth_path[i + 1]; // Calcul de l'angle entre (x, y) et (next_x, next_y) float delta_x = next_x * cell_size_mm_x - wp.x; float delta_y = (grid_height_ - 1 - next_y) * cell_size_mm_y - wp.y; wp.theta = std::atan2(delta_y, delta_x); // Calcul de l'angle en radians - } - else - { + } else { + wp.x = goal->x; + wp.y = goal->y; wp.theta = goal->theta; } @@ -537,26 +480,22 @@ namespace Modelec wp.is_end = false; waypoints.push_back(wp); } - if (!waypoints.empty()) - { + if (!waypoints.empty()) { waypoints.back().is_end = true; } return {FREE, waypoints}; } - void Pathfinding::SetCurrentPos(const PosMsg::SharedPtr& pos) - { + void Pathfinding::SetCurrentPos(const PosMsg::SharedPtr &pos) { current_start_ = pos; } - std::shared_ptr Pathfinding::GetObstacle(int id) const - { + std::shared_ptr Pathfinding::GetObstacle(int id) const { return obstacle_map_.at(id); } - void Pathfinding::RemoveObstacle(int id) - { + void Pathfinding::RemoveObstacle(int id) { obstacle_map_.erase(id); modelec_interfaces::msg::Obstacle msg; @@ -564,38 +503,42 @@ namespace Modelec obstacle_remove_pub_->publish(msg); } - void Pathfinding::AddObstacle(const std::shared_ptr& obstacle) - { + void Pathfinding::AddObstacle(const std::shared_ptr &obstacle) { obstacle_map_[obstacle->GetId()] = obstacle; modelec_interfaces::msg::Obstacle msg = obstacle->toMsg(); obstacle_add_pub_->publish(msg); } - std::shared_ptr Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos, - const std::vector& blacklistedId) - { - // TODO score (count dist and dist with enemy) + std::shared_ptr Pathfinding::GetClosestColumn(const PosMsg::SharedPtr &pos, + const std::vector &blacklistedId) { std::shared_ptr closest_obstacle = nullptr; auto robotPos = Point(pos->x, pos->y, pos->theta); auto enemyPos = Point(last_enemy_pos_.x, last_enemy_pos_.y, last_enemy_pos_.theta); float score = std::numeric_limits::max(); - for (const auto& [id, obstacle] : obstacle_map_) - { - if (auto obs = std::dynamic_pointer_cast(obstacle)) - { + for (const auto &[id, obstacle]: obstacle_map_) { + if (auto obs = std::dynamic_pointer_cast(obstacle)) { if (!obs->IsAtObjective() && std::find(blacklistedId.begin(), blacklistedId.end(), obs->GetId()) == - blacklistedId.end()) - { - for (auto possiblePos : obs->GetAllPossiblePositions()) - { + blacklistedId.end()) { + for (auto possiblePos: obs->GetAllPossiblePositions()) { auto dist = Point::distance(robotPos, possiblePos); auto distEnemy = Point::distance(enemyPos, possiblePos); + auto takePoint = possiblePos; + takePoint.theta += M_PI; + double theta = std::abs(Point::angleDiff(robotPos, takePoint)); - auto s = dist + distEnemy * factor_close_enemy_; + auto dist_score = dist; + auto distEnemy_score = distEnemy * factor_close_enemy_ * has_enemy_pos_; + auto theta_score = (theta / M_PI * 250); + + auto s = dist_score + distEnemy_score + theta_score; + + if (s < score) { + /*RCLCPP_INFO(node_->get_logger(), + "Column %d at (%d, %d) with dist_s=%f, distEnemy_s=%f, theta=%f, theta_s=%f, score=%f", + obs->GetId(), possiblePos.x, possiblePos.y, dist_score, distEnemy_score, theta, + theta_score, s);*/ - if (s < score) - { score = s; closest_obstacle = obs; } @@ -608,63 +551,50 @@ namespace Modelec } void Pathfinding::HandleMapRequest(const std::shared_ptr, - const std::shared_ptr response) - { + const std::shared_ptr response) { response->width = grid_[0].size(); response->height = grid_.size(); response->map = std::vector(grid_.size() * grid_[0].size()); - for (size_t i = 0; i < grid_.size(); i++) - { - for (size_t j = 0; j < grid_[i].size(); j++) - { + for (size_t i = 0; i < grid_.size(); i++) { + for (size_t j = 0; j < grid_[i].size(); j++) { response->map[i * grid_[i].size() + j] = grid_[i][j]; } } } void Pathfinding::HandleMapSizeRequest(const std::shared_ptr, - const std::shared_ptr response) - { + const std::shared_ptr response) { response->width = grid_width_; response->height = grid_height_; } void Pathfinding::HandleAskObstacleRequest(const std::shared_ptr, - const std::shared_ptr) - { - for (auto& [index, obs] : obstacle_map_) - { + const std::shared_ptr) { + for (auto &[index, obs]: obstacle_map_) { obstacle_add_pub_->publish(obs->toMsg()); } } - void Pathfinding::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) - { + void Pathfinding::OnEnemyPosition(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { last_enemy_pos_ = *msg; has_enemy_pos_ = true; } - void Pathfinding::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) - { + void Pathfinding::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) { Point enemyPos(msg->x, msg->y, msg->theta); - for (auto& [index, obs] : obstacle_map_) - { - if (auto column = std::dynamic_pointer_cast(obs)) - { + for (auto &[index, obs]: obstacle_map_) { + if (auto column = std::dynamic_pointer_cast(obs)) { if (Point::distance(enemyPos, column->GetPosition()) < enemy_width_mm_ + (column->GetWidth() / 2) + - enemy_margin_mm_) - { + enemy_margin_mm_) { RemoveObstacle(column->GetId()); } } } } - bool Pathfinding::TestCollision(int x, int y, int collisionMask) - { + bool Pathfinding::TestCollision(int x, int y, int collisionMask) { if (y < 0 || y >= static_cast(grid_.size()) || - x < 0 || x >= static_cast(grid_[y].size())) - { + x < 0 || x >= static_cast(grid_[y].size())) { RCLCPP_WARN(node_->get_logger(), "TestCollision: access out of bounds x=%d y=%d", x, y); return false; // ou true, selon ce que tu veux (false = pas de collision) } @@ -672,34 +602,29 @@ namespace Modelec return (grid_[y][x] & collisionMask) && !(grid_[y][x] & ~collisionMask); } - bool Pathfinding::LoadObstaclesFromXML(const std::string& filename) - { + bool Pathfinding::LoadObstaclesFromXML(const std::string &filename) { tinyxml2::XMLDocument doc; - if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) - { + if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS) { RCLCPP_ERROR(node_->get_logger(), "Failed to load obstacle XML file: %s", filename.c_str()); return false; } - tinyxml2::XMLElement* root = doc.FirstChildElement("Obstacles"); - if (!root) - { + tinyxml2::XMLElement *root = doc.FirstChildElement("Obstacles"); + if (!root) { RCLCPP_ERROR(node_->get_logger(), "No root element in file"); return false; } - for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Obstacle"); + for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Obstacle"); obstacleElem != nullptr; - obstacleElem = obstacleElem->NextSiblingElement("Obstacle")) - { + obstacleElem = obstacleElem->NextSiblingElement("Obstacle")) { std::shared_ptr obs = std::make_shared(obstacleElem); obstacle_map_[obs->GetId()] = obs; } - for (tinyxml2::XMLElement* obstacleElem = root->FirstChildElement("Gradin"); + for (tinyxml2::XMLElement *obstacleElem = root->FirstChildElement("Gradin"); obstacleElem != nullptr; - obstacleElem = obstacleElem->NextSiblingElement("Gradin")) - { + obstacleElem = obstacleElem->NextSiblingElement("Gradin")) { std::shared_ptr obs = std::make_shared(obstacleElem); obstacle_map_[obs->GetId()] = obs; } @@ -708,8 +633,7 @@ namespace Modelec return true; } - Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos& pos, int index, bool isLast) - { + Waypoint::Waypoint(const modelec_interfaces::msg::OdometryPos &pos, int index, bool isLast) { id = index; x = pos.x; y = pos.y; @@ -718,8 +642,7 @@ namespace Modelec reached = false; } - Waypoint::Waypoint(const WaypointMsg& waypoint) - { + Waypoint::Waypoint(const WaypointMsg &waypoint) { id = waypoint.id; x = waypoint.x; y = waypoint.y; @@ -728,8 +651,7 @@ namespace Modelec reached = false; } - WaypointMsg Waypoint::ToMsg() const - { - return static_cast>>(*this); + WaypointMsg Waypoint::ToMsg() const { + return static_cast >>(*this); } -} +} \ No newline at end of file diff --git a/src/modelec_utils/include/modelec_utils/point.hpp b/src/modelec_utils/include/modelec_utils/point.hpp index 70ef661..ebbe2c9 100644 --- a/src/modelec_utils/include/modelec_utils/point.hpp +++ b/src/modelec_utils/include/modelec_utils/point.hpp @@ -11,11 +11,12 @@ namespace Modelec Point(int x, int y, double theta) : x(x), y(y), theta(theta) {} static double distance(const Point& p1, const Point& p2); + static double angleDiff(const Point& p1, const Point& p2); - Point GetTakePosition(int distance, double angle) const; - Point GetTakePosition(int distance) const; + [[nodiscard]] Point GetTakePosition(int distance, double angle) const; + [[nodiscard]] Point GetTakePosition(int distance) const; - Point GetTakeBasePosition() const; - Point GetTakeClosePosition() const; + [[nodiscard]] Point GetTakeBasePosition() const; + [[nodiscard]] Point GetTakeClosePosition() const; }; } diff --git a/src/modelec_utils/src/point.cpp b/src/modelec_utils/src/point.cpp index 7e43d7f..bfa8888 100644 --- a/src/modelec_utils/src/point.cpp +++ b/src/modelec_utils/src/point.cpp @@ -8,6 +8,13 @@ namespace Modelec return sqrt(std::pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2)); } + double Point::angleDiff(const Point &p1, const Point &p2) { + double diff = std::fmod(p1.theta - p2.theta + M_PI, 2 * M_PI); + if (diff < 0) + diff += 2 * M_PI; + return diff - M_PI; + } + Point Point::GetTakePosition(int distance, double angle) const { Point pos;