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

some patch

Browse Source
This commit is contained in:
acki
2025-05-28 10:36:57 -04:00
parent 6a22e0c44a
commit c6e8c7cf4e
10 changed files with 83 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -25,6 +25,8 @@
#include <modelec_interfaces/srv/odometry_set_pid.hpp>
#include <modelec_interfaces/srv/odometry_add_waypoint.hpp>
#include <std_msgs/msg/bool.hpp>
namespace Modelec
{
class PCBOdoInterface : public rclcpp::Node
@@ -67,9 +69,9 @@ namespace Modelec
rclcpp::Subscription<modelec_interfaces::msg::OdometryPos>::SharedPtr odo_set_pos_subscriber_;
rclcpp::Subscription<modelec_interfaces::msg::OdometryPid>::SharedPtr odo_set_pid_subscriber_;
void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const;
void SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
void SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const;
void AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::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_;
@@ -79,6 +81,9 @@ namespace Modelec
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;
// Promises and mutexes to synchronize service responses asynchronously
std::queue<std::promise<long>> tof_promises_;
std::mutex tof_mutex_;
@@ -138,28 +143,28 @@ namespace Modelec
bool isOk = false;
public:
void SendToPCB(const std::string& data) const;
void SendToPCB(const std::string& data);
void SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data = {}) const;
const std::vector<std::string>& data = {});
void GetData(const std::string& elem, const std::vector<std::string>& data = {}) const;
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {}) const;
void GetData(const std::string& elem, const std::vector<std::string>& data = {});
void SendOrder(const std::string& elem, const std::vector<std::string>& data = {});
void GetPos() const;
void GetSpeed() const;
void GetToF(const int& tof) const;
void GetPos();
void GetSpeed();
void GetToF(const int& tof);
void SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const;
void SetRobotPos(long x, long y, double theta) const;
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) const;
void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta) const;
void AddWaypoint(modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg);
void AddWaypoint(int index, bool IsStopPoint, long x, long y, double theta);
void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const;
void SetStart(bool start) const;
void SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg);
void SetStart(bool start);
void GetPID() const;
void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const;
void SetPID(float p, float i, float d) const;
void GetPID();
void SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg);
void SetPID(float p, float i, float d);
};
} // namespace Modelec

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

@@ -170,6 +170,14 @@ namespace Modelec
{
HandleAddWaypoint(request, response);
});
start_odo_sub_ = this->create_subscription<std_msgs::msg::Bool>(
"odometry/start", 10,
[this](const std_msgs::msg::Bool::SharedPtr msg)
{
start_odo_ = msg->data;
SendOrder("START", {std::to_string(msg->data)});
});
}
PCBOdoInterface::~PCBOdoInterface()
@@ -313,17 +321,17 @@ namespace Modelec
}
}
void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const
void PCBOdoInterface::AddWaypointCallback(const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
{
AddWaypoint(msg);
}
void PCBOdoInterface::SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const
void PCBOdoInterface::SetPosCallback(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
SetRobotPos(msg);
}
void PCBOdoInterface::SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const
void PCBOdoInterface::SetPIDCallback(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
{
SetPID(msg);
}
@@ -603,7 +611,7 @@ namespace Modelec
}
}
void PCBOdoInterface::SendToPCB(const std::string& data) const
void PCBOdoInterface::SendToPCB(const std::string& data)
{
if (pcb_publisher_)
{
@@ -614,7 +622,7 @@ namespace Modelec
}
void PCBOdoInterface::SendToPCB(const std::string& order, const std::string& elem,
const std::vector<std::string>& data) const
const std::vector<std::string>& data)
{
std::string command = order + ";" + elem;
for (const auto& d : data)
@@ -626,37 +634,37 @@ namespace Modelec
SendToPCB(command);
}
void PCBOdoInterface::GetData(const std::string& elem, const std::vector<std::string>& data) const
void PCBOdoInterface::GetData(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("GET", elem, data);
}
void PCBOdoInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data) const
void PCBOdoInterface::SendOrder(const std::string& elem, const std::vector<std::string>& data)
{
SendToPCB("SET", elem, data);
}
void PCBOdoInterface::GetPos() const
void PCBOdoInterface::GetPos()
{
GetData("POS");
}
void PCBOdoInterface::GetSpeed() const
void PCBOdoInterface::GetSpeed()
{
GetData("SPEED");
}
void PCBOdoInterface::GetToF(const int& tof) const
void PCBOdoInterface::GetToF(const int& tof)
{
GetData("DIST", {std::to_string(tof)});
}
void PCBOdoInterface::SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg) const
void PCBOdoInterface::SetRobotPos(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)
{
SetRobotPos(msg->x, msg->y, msg->theta);
}
void PCBOdoInterface::SetRobotPos(const long x, const long y, const double theta) const
void PCBOdoInterface::SetRobotPos(const long x, const long y, const double theta)
{
std::vector<std::string> data = {
std::to_string(x),
@@ -668,14 +676,20 @@ namespace Modelec
}
void PCBOdoInterface::AddWaypoint(
const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg) const
const modelec_interfaces::msg::OdometryAddWaypoint::SharedPtr msg)
{
AddWaypoint(msg->id, msg->is_end, msg->x, msg->y, msg->theta);
}
void PCBOdoInterface::AddWaypoint(const int index, const bool IsStopPoint, const long x, const long y,
const double theta) const
const double theta)
{
if (!start_odo_)
{
SendOrder("START", {std::to_string(true)});
start_odo_ = true;
}
std::vector<std::string> data = {
std::to_string(index),
std::to_string(IsStopPoint),
@@ -687,27 +701,27 @@ namespace Modelec
SendOrder("WAYPOINT", data);
}
void PCBOdoInterface::SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg) const
void PCBOdoInterface::SetStart(const modelec_interfaces::msg::OdometryStart::SharedPtr msg)
{
SetStart(msg->start);
}
void PCBOdoInterface::SetStart(bool start) const
void PCBOdoInterface::SetStart(bool start)
{
SendOrder("START", {std::to_string(start)});
}
void PCBOdoInterface::GetPID() const
void PCBOdoInterface::GetPID()
{
GetData("PID");
}
void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg) const
void PCBOdoInterface::SetPID(const modelec_interfaces::msg::OdometryPid::SharedPtr msg)
{
SetPID(msg->p, msg->i, msg->d);
}
void PCBOdoInterface::SetPID(float p, float i, float d) const
void PCBOdoInterface::SetPID(float p, float i, float d)
{
std::vector<std::string> data = {
std::to_string(p),

4
src/modelec_strat/data/config.xml
View File

@@ -10,6 +10,7 @@
<refresh_rate>0.5</refresh_rate>
<max_stationary_time_s>5</max_stationary_time_s>
<min_emergency_distance_mm>500</min_emergency_distance_mm>
<margin_detection_table_mm>50</margin_detection_table_mm>
</detection>
<factor_close_enemy>-0.3</factor_close_enemy>
</enemy>
@@ -55,16 +56,19 @@
<pcb_odo>
<name>pcb_odo</name>
<port>/dev/USB_ODO</port>
<!--<port>/tmp/USB_ODO</port>-->
<baud_rate>115200</baud_rate>
</pcb_odo>
<pcb_alim>
<name>pcb_alim</name>
<port>/dev/USB_ALIM</port>
<!--<port>/tmp/USB_ALIM</port>-->
<baud_rate>115200</baud_rate>
</pcb_alim>
<pcb_action>
<name>pcb_action</name>
<port>/dev/USB_ACTION</port>
<!--<port>/tmp/USB_ACTION</port>-->
<baud_rate>115200</baud_rate>
</pcb_action>
</pcb>

1
src/modelec_strat/include/modelec_strat/enemy_manager.hpp
View File

@@ -44,6 +44,7 @@ namespace Modelec
float max_stationary_time_s_ = 10.0f;
float map_width_ = 0;
float map_height_ = 0;
float margin_detection_table_ = 0.0f;
float robot_width_ = 0;
float robot_length_ = 0;
double robot_radius_ = 0;

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

@@ -8,6 +8,8 @@
#include <modelec_interfaces/msg/spawn.hpp>
#include <std_srvs/srv/empty.hpp>
#include <std_msgs/msg/bool.hpp>
#include "deposite_zone.hpp"
#include "pathfinding.hpp"
@@ -95,8 +97,6 @@ namespace Modelec
void SetSpawn(const std::string& name);
Point GetSpawnYellow() const;
Point GetSpawnBlue() const;
Point GetSpawn() const;
protected:
@@ -144,6 +144,8 @@ namespace Modelec
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_;
modelec_interfaces::msg::OdometryPos last_enemy_pos_;
bool await_rotate_ = false;

2
src/modelec_strat/include/modelec_strat/strat_fms.hpp
View File

@@ -75,6 +75,6 @@ namespace Modelec
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedPtr client_start_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;
};
}

9
src/modelec_strat/src/enemy_manager.cpp
View File

@@ -22,6 +22,8 @@ namespace Modelec
map_width_ = Config::get<float>("config.map.size.map_width_mm", 3000.0);
map_height_ = Config::get<float>("config.map.size.map_height_mm", 2000.0);
margin_detection_table_ = Config::get<float>("config.enemy.detection.margin_detection_table_mm", 50.0);
robot_width_ = Config::get<float>("config.robot.size.width_mm", 500.0);
robot_length_ = Config::get<float>("config.robot.size.length_mm", 500.0);
robot_radius_ = std::max(robot_width_, robot_length_) / 2.0;
@@ -134,7 +136,7 @@ namespace Modelec
double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
// Check if in bounds
if (x_global >= 0 && x_global <= map_width_ && y_global >= 0 && y_global <= map_height_)
if (x_global >= 0 && x_global <= (map_width_ - margin_detection_table_) && y_global >= 0 && y_global <= (map_height_ - margin_detection_table_))
{
modelec_interfaces::msg::OdometryPos emergency_msg;
emergency_msg.x = x_global;
@@ -145,8 +147,7 @@ namespace Modelec
RCLCPP_WARN(this->get_logger(), "EMERGENCY CLOSE OBJECT DETECTED at x=%.2f y=%.2f (%.1f mm)", x_global, y_global, range_mm);
}
angle += msg->angle_increment;
continue;
break;;
}
// Convert to local robot frame
@@ -158,7 +159,7 @@ namespace Modelec
double y_global = robot_y + (x_local * std::sin(robot_theta) + y_local * std::cos(robot_theta));
// Ignore points outside of the table
if (x_global < 0 || x_global > map_width_ || y_global < 0 || y_global > map_height_)
if (x_global < 0 || x_global > (map_width_ - margin_detection_table_) || y_global < 0 || y_global > (map_height_ - margin_detection_table_))
{
// RCLCPP_INFO(this->get_logger(), "Lidar point out of bounds: x=%.2f, y=%.2f", x_global, y_global);

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

@@ -63,6 +63,8 @@ namespace Modelec
OnEnemyPositionLongTime(msg);
});
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";
if (!LoadDepositeZoneFromXML(deposite_zone_path))
@@ -531,17 +533,9 @@ namespace Modelec
pathfinding_->OnEnemyPosition(msg);
last_enemy_pos_ = *msg;
waypoints_.clear();
WaypointMsg emptyWaypoint;
emptyWaypoint.x = current_pos_->x;
emptyWaypoint.y = current_pos_->y;
emptyWaypoint.theta = current_pos_->theta;
emptyWaypoint.id = 0;
emptyWaypoint.is_end = true;
waypoints_.emplace_back(emptyWaypoint);
SendWaypoint();
std_msgs::msg::Bool start_odo_msg;
start_odo_msg.data = false;
start_odo_pub_->publish(start_odo_msg);
}
void NavigationHelper::OnEnemyPositionLongTime(const modelec_interfaces::msg::OdometryPos::SharedPtr msg)

22
src/modelec_strat/src/strat_fms.cpp
View File

@@ -48,23 +48,7 @@ namespace Modelec
setup_ = true;
});
client_start_ = create_client<modelec_interfaces::srv::OdometryStart>("/odometry/start");
while (!client_start_->wait_for_service(std::chrono::seconds(1))) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(get_logger(), "Interrupted while waiting for the service. Exiting.");
return;
}
RCLCPP_INFO(get_logger(), "Service not available, waiting again...");
}
auto request = std::make_shared<modelec_interfaces::srv::OdometryStart::Request>();
request->start = true;
client_start_->async_send_request(request, [this](rclcpp::Client<modelec_interfaces::srv::OdometryStart>::SharedFuture response) {
if (!response.get()->success)
{
RCLCPP_ERROR(get_logger(), "Failed to send start command.");
}
});
start_odo_pub_ = create_publisher<std_msgs::msg::Bool>("/odometry/start", 10);
std::string config_path = ament_index_cpp::get_package_share_directory("modelec_strat") + "/data/config.xml";
if (!Config::load(config_path))
@@ -134,6 +118,10 @@ namespace Modelec
case State::INIT:
if (setup_ && team_selected_)
{
std_msgs::msg::Bool start_odo_msg;
start_odo_msg.data = true;
start_odo_pub_->publish(start_odo_msg);
Transition(State::WAIT_START, "System ready");
}
break;

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

@@ -29,6 +29,6 @@ namespace Modelec
Point Point::GetTakeClosePosition() const
{
return GetTakePosition(130, theta);
return GetTakePosition(150, theta);
}
}