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

up to last odo version

Browse Source
This commit is contained in:
acki
2025-10-03 13:26:40 +02:00
parent 06afa03c2b
commit 8c9e6e67ad
33 changed files with 470 additions and 1285 deletions
Download Patch File Download Diff File Expand all files Collapse all files

20
install.sh
View File

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

7
simulated_pcb/action.py
View File

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

30
simulated_pcb/odo.py
View File

@@ -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(';')

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

@@ -11,8 +11,8 @@
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_speed.hpp>
#include <modelec_interfaces/msg/odometry_to_f.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoints.hpp>
#include <modelec_interfaces/msg/odometry_start.hpp>
#include <modelec_interfaces/msg/odometry_pid.hpp>
@@ -64,83 +64,22 @@ namespace Modelec
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::OdometryWaypointReach>::SharedPtr odo_waypoint_reach_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::OdometryAddWaypoint>::SharedPtr odo_add_waypoint_subscriber_;
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::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<modelec_interfaces::srv::OdometryToF>::SharedPtr get_tof_service_;
rclcpp::Service<modelec_interfaces::srv::OdometrySpeed>::SharedPtr get_speed_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryPosition>::SharedPtr get_position_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryStart>::SharedPtr set_start_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryGetPid>::SharedPtr get_pid_service_;
rclcpp::Service<modelec_interfaces::srv::OdometrySetPid>::SharedPtr set_pid_service_;
rclcpp::Service<modelec_interfaces::srv::OdometryAddWaypoint>::SharedPtr add_waypoint_service_;
rclcpp::Subscription<std_msgs::msg::Bool>::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<std::promise<long>> tof_promises_;
std::mutex tof_mutex_;
std::queue<std::promise<OdometryData>> speed_promises_;
std::mutex speed_mutex_;
std::queue<std::promise<OdometryData>> pos_promises_;
std::mutex pos_mutex_;
std::queue<std::promise<bool>> start_promises_;
std::mutex start_mutex_;
std::queue<std::promise<PIDData>> get_pid_promises_;
std::mutex get_pid_mutex_;
std::queue<std::promise<bool>> set_pid_promises_;
std::mutex set_pid_mutex_;
std::queue<std::promise<bool>> add_waypoint_promises_;
std::mutex add_waypoint_mutex_;
// Service handlers using async wait with promises
void HandleGetTof(const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response);
void HandleGetSpeed(const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response);
void HandleGetPosition(const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response);
void HandleGetStart(const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response);
void HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response);
void HandleSetPID(const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response);
void HandleAddWaypoint(const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response);
// Resolving methods called by subscriber callback
void ResolveToFRequest(long distance);
void ResolveSpeedRequest(const OdometryData& speed);
void ResolvePositionRequest(const OdometryData& position);
void ResolveStartRequest(bool start);
void ResolveGetPIDRequest(const PIDData& pid);
void ResolveSetPIDRequest(bool success);
void ResolveAddWaypointRequest(bool success);
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

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

@@ -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<std::string>("serial_port", "/dev/ttyUSB0");
declare_parameter<int>("baudrate", 115200);
declare_parameter<std::string>("name", "pcb_action");
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = Config::get<std::string>("config.usb.pcb.pcb_alim.name", "pcb_odo");
request->bauds = Config::get<int>("config.usb.pcb.pcb_alim.baudrate", 115200);
request->serial_port = Config::get<std::string>("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<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))

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

@@ -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<std::string>("serial_port", "/dev/USB_ODO");
declare_parameter<int>("baudrate", 115200);
declare_parameter<std::string>("name", "pcb_action");
// Service to create a new serial listener
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = Config::get<std::string>("config.usb.pcb.pcb_odo.name", "pcb_odo");
request->bauds = Config::get<int>("config.usb.pcb.pcb_odo.baudrate", 115200);
request->serial_port = Config::get<std::string>("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<modelec_interfaces::srv::AddSerialListener>("add_serial_listener");
while (!client->wait_for_service(std::chrono::seconds(1)))
@@ -96,21 +94,26 @@ namespace Modelec
odo_tof_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryToF>(
"odometry/tof", 10);
odo_waypoint_reach_publisher_ = this->create_publisher<modelec_interfaces::msg::OdometryWaypointReach>(
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::OdometryAddWaypoint>(
odo_add_waypoint_subscriber_ = this->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
"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<modelec_interfaces::msg::OdometryWaypoints>(
"odometry/add_waypoint", 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)
@@ -125,63 +128,6 @@ namespace Modelec
SetPIDCallback(msg);
});
// Services
get_tof_service_ = create_service<modelec_interfaces::srv::OdometryToF>(
"odometry/tof",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
{
HandleGetTof(request, response);
});
get_speed_service_ = create_service<modelec_interfaces::srv::OdometrySpeed>(
"odometry/speed",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
{
HandleGetSpeed(request, response);
});
get_position_service_ = create_service<modelec_interfaces::srv::OdometryPosition>(
"odometry/get_position",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
{
HandleGetPosition(request, response);
});
set_start_service_ = create_service<modelec_interfaces::srv::OdometryStart>(
"odometry/start",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::Response> response)
{
HandleGetStart(request, response);
});
get_pid_service_ = create_service<modelec_interfaces::srv::OdometryGetPid>(
"odometry/get_pid",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
{
HandleGetPID(request, response);
});
set_pid_service_ = create_service<modelec_interfaces::srv::OdometrySetPid>(
"odometry/set_pid",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::Response> response)
{
HandleSetPID(request, response);
});
add_waypoint_service_ = create_service<modelec_interfaces::srv::OdometryAddWaypoint>(
"odometry/add_waypoint",
[this](const std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::Response> response)
{
HandleAddWaypoint(request, response);
});
start_odo_sub_ = this->create_subscription<std_msgs::msg::Bool>(
"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<modelec_interfaces::srv::OdometryToF::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryToF::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
return;
}
std::promise<long> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> lock(tof_mutex_);
tof_promises_.push(std::move(promise));
}
GetToF(request->n);
response->distance = future.get();
}
void PCBOdoInterface::HandleGetSpeed(
const std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometrySpeed::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
return;
}
std::promise<OdometryData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> 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<modelec_interfaces::srv::OdometryPosition::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometryPosition::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
return;
}
std::promise<OdometryData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> 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<modelec_interfaces::srv::OdometryStart::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryStart::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(start_mutex_);
start_promises_.push(std::move(promise));
}
SetStart(request->start);
response->success = future.get();
}
void PCBOdoInterface::HandleGetPID(const std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Request>,
std::shared_ptr<modelec_interfaces::srv::OdometryGetPid::Response> response)
{
if (!isOk)
{
RCLCPP_ERROR(get_logger(), "PCB not initialized");
return;
}
std::promise<PIDData> promise;
auto future = promise.get_future();
{
std::lock_guard<std::mutex> 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<modelec_interfaces::srv::OdometrySetPid::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometrySetPid::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(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<modelec_interfaces::srv::OdometryAddWaypoint::Request> request,
std::shared_ptr<modelec_interfaces::srv::OdometryAddWaypoint::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(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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::string> 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<std::string> data = {
name,
std::to_string(p),
std::to_string(i),
std::to_string(d)

9
src/modelec_core/CMakeLists.txt
View File

@@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
add_executable(speed_result src/speed_result.cpp)
ament_target_dependencies(speed_result rclcpp std_msgs sensor_msgs modelec_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}
)

65
src/modelec_core/include/modelec/gamecontroller_listener.hpp
View File

@@ -1,65 +0,0 @@
#pragma once
#include "modelec_interfaces/msg/pca9685_servo.hpp"
#include <array>
#include <rclcpp/publisher.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joy.hpp>
#include <modelec_interfaces/msg/servo_mode.hpp>
#include <modelec_interfaces/msg/pca9685_servo.hpp>
#include <modelec_interfaces/srv/add_servo_motor.hpp>
namespace Modelec {
class ControllerListener : public rclcpp::Node
{
using ServoMode = modelec_interfaces::msg::ServoMode;
std::array<int, 3> 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<SolarPannelServo, 2> 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<sensor_msgs::msg::Joy>::SharedPtr subscription_;
rclcpp::Publisher<ServoMode>::SharedPtr servo_publisher_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr odometry_publisher_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr clear_pca_publisher_;
rclcpp::Publisher<modelec_interfaces::msg::PCA9685Servo>::SharedPtr pca9685_publisher_;
rclcpp::Client<modelec_interfaces::srv::AddServoMotor>::SharedPtr client_;
void CheckButton(const sensor_msgs::msg::Joy::SharedPtr msg);
void CheckAxis(const sensor_msgs::msg::Joy::SharedPtr msg);
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;
};
}

310
src/modelec_core/src/gamecontroller_listener.cpp
View File

@@ -1,310 +0,0 @@
#include "modelec/gamecontroller_listener.hpp"
#include "modelec_utils/utils.hpp"
#include <modelec_interfaces/srv/add_serial_listener.hpp>
namespace Modelec
{
ControllerListener::ControllerListener() : Node("controller_listener")
{
// Subscribe to the 'joy' topic
subscription_ = this->create_subscription<sensor_msgs::msg::Joy>(
"joy", 10,
std::bind(&ControllerListener::joy_callback, this, std::placeholders::_1));
servo_publisher_ = this->create_publisher<ServoMode>("arm_control", 10);
// Service to create a new serial listener
auto request = std::make_shared<modelec_interfaces::srv::AddSerialListener::Request>();
request->name = "odometry";
request->bauds = 115200;
request->serial_port = "/dev/ttyACM0";
auto client = this->create_client<modelec_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)
{
RCLCPP_INFO(this->get_logger(), "Serial listener created");
odometry_publisher_ = this->create_publisher<std_msgs::msg::String>(result.get()->subscriber, 10);
clear_pca_publisher_ = this->create_publisher<std_msgs::msg::Empty>("clear_pca9685", 10);
pca9685_publisher_ = this->create_publisher<modelec_interfaces::msg::PCA9685Servo>(
"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<modelec_interfaces::srv::AddServoMotor>("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<modelec_interfaces::srv::AddServoMotor::Request>();
request->pin = servo.pin;
auto res = client_->async_send_request(request);
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), res) ==
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<int>(Modelec::mapValue(static_cast<float>(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<int>(Modelec::mapValue(static_cast<float>(-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<int>(Modelec::mapValue(static_cast<float>(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<int>(Modelec::mapValue(static_cast<float>(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<Modelec::ControllerListener>());
rclcpp::shutdown();
return 0;
}

7
src/modelec_gui/include/modelec_gui/pages/map_page.hpp
View File

@@ -12,12 +12,12 @@
#include <std_srvs/srv/empty.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoints.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/srv/map.hpp>
#include <modelec_interfaces/srv/map_size.hpp>
#include <modelec_interfaces/msg/obstacle.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/strat_state.hpp>
#include <std_msgs/msg/bool.hpp>
@@ -78,7 +78,8 @@ namespace ModelecGUI {
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr add_waypoint_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr add_waypoints_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr score_sub_;

12
src/modelec_gui/include/modelec_gui/pages/odo_page.hpp
View File

@@ -9,7 +9,7 @@
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoint.hpp>
#include <modelec_interfaces/srv/odometry_speed.hpp>
#include <modelec_interfaces/srv/odometry_start.hpp>
#include <modelec_interfaces/srv/odometry_get_pid.hpp>
@@ -45,13 +45,13 @@ namespace ModelecGUI
rclcpp::Node::SharedPtr node_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr sub_;
// rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr sub_;
// client
rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedPtr client_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedPtr client_get_pid_;
rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::SharedPtr client_set_pid_;
/* rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::SharedPtr client_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::SharedPtr client_get_pid_;
rclcpp::Client<modelec_interfaces::srv::OdometrySetPid>::SharedPtr client_set_pid_;*/
void PositionCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg);
};

9
src/modelec_gui/include/modelec_gui/pages/test_map_page.hpp
View File

@@ -12,13 +12,13 @@
#include <std_srvs/srv/empty.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoints.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_go_to.hpp>
#include <modelec_interfaces/srv/map.hpp>
#include <modelec_interfaces/srv/map_size.hpp>
#include <modelec_interfaces/msg/obstacle.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/strat_state.hpp>
#include <std_msgs/msg/bool.hpp>
@@ -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<modelec_interfaces::msg::OdometryAddWaypoint>::SharedPtr add_waypoint_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoint>::SharedPtr add_waypoint_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryWaypoints>::SharedPtr add_waypoints_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odometry_sub_;
rclcpp::Publisher<modelec_interfaces::msg::OdometryGoTo>::SharedPtr go_to_pub_;

24
src/modelec_gui/src/modelec_gui.cpp
View File

@@ -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]() {

12
src/modelec_gui/src/pages/home_page.cpp
View File

@@ -2,9 +2,7 @@
#include <modelec_gui/pages/home_page.hpp>
#include <QVBoxLayout>
#include <modelec_interfaces/msg/detail/servo_mode__builder.hpp>
#include "../../../modelec_utils/include/modelec_utils/config.hpp"
#include <modelec_utils/config.hpp>
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<modelec_interfaces::msg::Spawn>("/strat/spawn", 10);
spawn_pub_ = node_->create_publisher<modelec_interfaces::msg::Spawn>("strat/spawn", 10);
auto w = Modelec::Config::get<int>("config.spawn.width_mm");
auto h = Modelec::Config::get<int>("config.spawn.height_mm");
spawn_sub_ = node_->create_subscription<modelec_interfaces::msg::Spawn>("/nav/spawn", 10,
spawn_sub_ = node_->create_subscription<modelec_interfaces::msg::Spawn>("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<std_msgs::msg::Empty>("/strat/reset", 10);
reset_strat_pub_ = node_->create_publisher<std_msgs::msg::Empty>("strat/reset", 10);
ask_spawn_client_ = node_->create_client<std_srvs::srv::Empty>("/nav/ask_spawn");
ask_spawn_client_ = node_->create_client<std_srvs::srv::Empty>("nav/ask_spawn");
ask_spawn_client_->wait_for_service();
auto ask_spawn_request_ = std::make_shared<std_srvs::srv::Empty::Request>();
auto res = ask_spawn_client_->async_send_request(ask_spawn_request_);

28
src/modelec_gui/src/pages/map_page.cpp
View File

@@ -51,9 +51,9 @@ namespace ModelecGUI
enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
"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<modelec_interfaces::msg::OdometryWaypoints>(
"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<modelec_interfaces::msg::OdometryPos>("odometry/position", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
@@ -111,7 +131,7 @@ namespace ModelecGUI
update();
});
strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("/strat/start_time", 10,
strat_start_sub_ = node_->create_subscription<std_msgs::msg::Int64>("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<modelec_interfaces::msg::StratState>("/strat/state", 10,
strat_state_sub_ = node_->create_subscription<modelec_interfaces::msg::StratState>("strat/state", 10,
[this](const modelec_interfaces::msg::StratState::SharedPtr msg)
{
if (msg->state == modelec_interfaces::msg::StratState::STOP)

26
src/modelec_gui/src/pages/odo_page.cpp
View File

@@ -13,7 +13,7 @@ namespace ModelecGUI
RCLCPP_INFO(node_->get_logger(), "Start button clicked.");
auto request = std::make_shared<modelec_interfaces::srv::OdometryStart::Request>();
request->start = true;
client_start_->async_send_request(
/*client_start_->async_send_request(
request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryStart>::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<modelec_interfaces::srv::OdometryGetPid::Request>();
// Make the asynchronous service call
client_get_pid_->async_send_request(
/*client_get_pid_->async_send_request(
request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryGetPid>::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<modelec_interfaces::srv::OdometrySetPid>::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<modelec_interfaces::srv::OdometrySpeed::Request>();
// Make the asynchronous service call
client_->async_send_request(
/*client_->async_send_request(
request, [this](rclcpp::Client<modelec_interfaces::srv::OdometrySpeed>::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<modelec_interfaces::msg::OdometryPos>(
"/odometry/position", 10,
std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));
/*sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"/odom/position", 10,
std::bind(&OdoPage::PositionCallback, this, std::placeholders::_1));*/
client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("odometry/speed");
/*client_ = node_->create_client<modelec_interfaces::srv::OdometrySpeed>("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;

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

@@ -25,17 +25,17 @@ namespace ModelecGUI
enemy_length_ = Modelec::Config::get<int>("config.enemy.size.length_mm", 300);
enemy_width_ = Modelec::Config::get<int>("config.enemy.size.width_mm", 300);
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryAddWaypoint>(
add_waypoint_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryWaypoint>(
"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<modelec_interfaces::msg::OdometryWaypoints>(
"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
{

79
src/modelec_interfaces/CMakeLists.txt
View File

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

13
src/modelec_interfaces/msg/Odometry/OdometryRealign.msg Normal file
View File

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

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

1
src/modelec_interfaces/msg/Odometry/OdometryWaypointReach.msg
View File

@@ -1 +0,0 @@
uint8 id

1
src/modelec_interfaces/msg/Odometry/OdometryWaypoints.msg Normal file
View File

@@ -0,0 +1 @@
OdometryWaypoint[] waypoints

6
src/modelec_interfaces/msg/Odometry/PID/OdometryPid.msg
View File

@@ -1,3 +1,9 @@
string POS="POS"
string THETA="THETA"
string LEFT="LEFT"
string RIGHT="RIGHT"
string name
float32 p
float32 i
float32 d

2
src/modelec_interfaces/msg/PCA9685Servo.msg
View File

@@ -1,2 +0,0 @@
uint8 pin
int64 angle

11
src/modelec_interfaces/msg/ServoMode.msg
View File

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

2
src/modelec_interfaces/msg/Solenoid.msg
View File

@@ -1,2 +0,0 @@
uint8 pin
bool state

18
src/modelec_strat/include/modelec_strat/navigation_helper.hpp
View File

@@ -1,8 +1,8 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/odometry_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoints.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/odometry_go_to.hpp>
#include <modelec_interfaces/msg/spawn.hpp>
@@ -41,6 +41,13 @@ namespace Modelec
// void SendWaypoint() const;
// void SendWaypoint(const std::vector<WaypointMsg>& waypoints) const;
void SendWaypoint() const;
void SendWaypoint(const std::vector<WaypointMsg>& waypoints) const;
void SendWaypoints() const;
void SendWaypoints(const std::vector<WaypointMsg>& 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<int, std::shared_ptr<DepositeZone>> deposite_zones_;
rclcpp::Subscription<WaypointReachMsg>::SharedPtr waypoint_reach_sub_;
rclcpp::Subscription<WaypointMsg>::SharedPtr waypoint_reach_sub_;
rclcpp::Publisher<WaypointMsg>::SharedPtr waypoint_pub_;
rclcpp::Publisher<WaypointsMsg>::SharedPtr waypoints_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryGoTo>::SharedPtr go_to_sub_;
rclcpp::Subscription<PosMsg>::SharedPtr pos_sub_;
rclcpp::Publisher<PosMsg>::SharedPtr pos_pub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr close_enemy_pos_sub_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr enemy_pos_long_time_sub_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;

12
src/modelec_strat/include/modelec_strat/pathfinding.hpp
View File

@@ -6,8 +6,8 @@
#include <tinyxml2.h>
#include <modelec_interfaces/msg/odometry_add_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoint_reach.hpp>
#include <modelec_interfaces/msg/odometry_waypoint.hpp>
#include <modelec_interfaces/msg/odometry_waypoints.hpp>
#include <modelec_interfaces/msg/odometry_pos.hpp>
#include <modelec_interfaces/msg/map.hpp>
#include <modelec_interfaces/srv/map.hpp>
@@ -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<WaypointMsg>;
@@ -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<int, WaypointListMsg> 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<Obstacle> GetObstacle(int id) const;
[[nodiscard]] std::shared_ptr<Obstacle> GetObstacle(int id) const;
void RemoveObstacle(int id);

129
src/modelec_strat/src/navigation_helper.cpp
View File

@@ -1,14 +1,12 @@
#include <modelec_strat/navigation_helper.hpp>
#include <utility>
#include <ament_index_cpp/get_package_share_directory.hpp>
#include "../../modelec_utils/include/modelec_utils/config.hpp"
#include <modelec_utils/config.hpp>
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<WaypointReachMsg>(
waypoint_reach_sub_ = node_->create_subscription<WaypointMsg>(
"odometry/waypoint_reach", 10,
[this](const WaypointReachMsg::SharedPtr msg)
[this](const WaypointMsg::SharedPtr msg)
{
OnWaypointReach(msg);
});
waypoint_pub_ = node_->create_publisher<WaypointMsg>("odometry/add_waypoint", 100);
waypoint_pub_ = node_->create_publisher<WaypointMsg>("odometry/add_waypoint", 30);
waypoints_pub_ = node_->create_publisher<WaypointsMsg>("odometry/add_waypoints", 30);
pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"odometry/position", 20,
@@ -51,21 +50,14 @@ namespace Modelec
OnEnemyPosition(msg);
});
close_enemy_pos_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"enemy/position/emergency", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
OnEnemyPositionClose(msg);
});
enemy_pos_long_time_sub_ = node_->create_subscription<modelec_interfaces::msg::OdometryPos>(
"/enemy/long_time", 10,
"enemy/long_time", 10,
[this](const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
OnEnemyPositionLongTime(msg);
});
start_odo_pub_ = node_->create_publisher<std_msgs::msg::Bool>("/odometry/start", 10);
start_odo_pub_ = node_->create_publisher<std_msgs::msg::Bool>("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<modelec_interfaces::msg::Spawn>("nav/spawn", 10);
ask_spawn_srv_ = node->create_service<std_srvs::srv::Empty>(
"/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<WaypointMsg>& 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<WaypointMsg>& 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<Obstacle> obs = std::make_shared<Obstacle>(
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)

328
src/modelec_strat/src/pathfinding.cpp
View File

@@ -3,33 +3,27 @@
#include <modelec_strat/obstacle/column.hpp>
#include <modelec_utils/config.hpp>
namespace Modelec
{
struct AStarNode
{
namespace Modelec {
struct AStarNode {
int x, y;
double g = std::numeric_limits<double>::infinity(); // Cost from start
double f = std::numeric_limits<double>::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<int>("config.map.size.map_width_mm", 3000);
map_height_mm_ = Config::get<int>("config.map.size.map_height_mm", 2000);
@@ -48,16 +42,14 @@ namespace Modelec
grid_height_ = Config::get<int>("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<modelec_interfaces::msg::Obstacle>(
"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<Obstacle>(*msg));
});
@@ -67,8 +59,7 @@ namespace Modelec
obstacle_remove_sub_ = node_->create_subscription<modelec_interfaces::msg::Obstacle>(
"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<std_srvs::srv::Empty>(
"nav/ask_map_obstacle",
[this](const std::shared_ptr<std_srvs::srv::Empty::Request> request,
const std::shared_ptr<std_srvs::srv::Empty::Response> response)
{
const std::shared_ptr<std_srvs::srv::Empty::Response> response) {
HandleAskObstacleRequest(request, response);
});
map_srv_ = node_->create_service<modelec_interfaces::srv::Map>(
"nav/map",
[this](const std::shared_ptr<modelec_interfaces::srv::Map::Request> request,
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response)
{
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response) {
HandleMapRequest(request, response);
});
map_size_srv_ = node_->create_service<modelec_interfaces::srv::MapSize>(
"nav/map_size",
[this](const std::shared_ptr<modelec_interfaces::srv::MapSize::Request> request,
const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response)
{
const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> 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<int, WaypointListMsg> Pathfinding::FindPath(const PosMsg::SharedPtr& start, const PosMsg::SharedPtr& goal,
bool isClose, int collisionMask)
{
if (!start || !goal)
{
std::pair<int, WaypointListMsg> 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<std::pair<float, float>> corners = {
std::vector<std::pair<float, float> > 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<AStarNode, std::vector<AStarNode>, std::greater<AStarNode>> open_list;
std::priority_queue<AStarNode, std::vector<AStarNode>, std::greater<AStarNode> > open_list;
std::unordered_map<int64_t, AStarNode> 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<std::pair<int, int>>{
auto neighbors = [](int x, int y) {
return std::vector<std::pair<int, int> >{
{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<std::pair<int, int>> path;
std::vector<std::pair<int, int> > 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<std::pair<int, int>> smooth_path;
std::vector<std::pair<int, int> > 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<Obstacle> Pathfinding::GetObstacle(int id) const
{
std::shared_ptr<Obstacle> 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>& obstacle)
{
void Pathfinding::AddObstacle(const std::shared_ptr<Obstacle> &obstacle) {
obstacle_map_[obstacle->GetId()] = obstacle;
modelec_interfaces::msg::Obstacle msg = obstacle->toMsg();
obstacle_add_pub_->publish(msg);
}
std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr& pos,
const std::vector<int>& blacklistedId)
{
// TODO score (count dist and dist with enemy)
std::shared_ptr<ColumnObstacle> Pathfinding::GetClosestColumn(const PosMsg::SharedPtr &pos,
const std::vector<int> &blacklistedId) {
std::shared_ptr<ColumnObstacle> 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<float>::max();
for (const auto& [id, obstacle] : obstacle_map_)
{
if (auto obs = std::dynamic_pointer_cast<ColumnObstacle>(obstacle))
{
for (const auto &[id, obstacle]: obstacle_map_) {
if (auto obs = std::dynamic_pointer_cast<ColumnObstacle>(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<modelec_interfaces::srv::Map::Request>,
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response)
{
const std::shared_ptr<modelec_interfaces::srv::Map::Response> response) {
response->width = grid_[0].size();
response->height = grid_.size();
response->map = std::vector<int>(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<modelec_interfaces::srv::MapSize::Request>,
const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response)
{
const std::shared_ptr<modelec_interfaces::srv::MapSize::Response> response) {
response->width = grid_width_;
response->height = grid_height_;
}
void Pathfinding::HandleAskObstacleRequest(const std::shared_ptr<std_srvs::srv::Empty::Request>,
const std::shared_ptr<std_srvs::srv::Empty::Response>)
{
for (auto& [index, obs] : obstacle_map_)
{
const std::shared_ptr<std_srvs::srv::Empty::Response>) {
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<ColumnObstacle>(obs))
{
for (auto &[index, obs]: obstacle_map_) {
if (auto column = std::dynamic_pointer_cast<ColumnObstacle>(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<int>(grid_.size()) ||
x < 0 || x >= static_cast<int>(grid_[y].size()))
{
x < 0 || x >= static_cast<int>(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 <Obstacles> 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<Obstacle> obs = std::make_shared<Obstacle>(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<ColumnObstacle> obs = std::make_shared<ColumnObstacle>(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<OdometryAddWaypoint_<std::allocator<void>>>(*this);
WaypointMsg Waypoint::ToMsg() const {
return static_cast<OdometryWaypoint_<std::allocator<void> >>(*this);
}
}

9
src/modelec_utils/include/modelec_utils/point.hpp
View File

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

7
src/modelec_utils/src/point.cpp
View File

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