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

pami protection

Browse Source
This commit is contained in:
acki
2025-05-07 05:49:01 -04:00
parent c7e4dc18e2
commit 0ca87e65a0
8 changed files with 216 additions and 25 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/modelec_core/launch/test.modelec.launch.yml
View File

@@ -1,31 +1,36 @@
launch:
- node:
pkg: 'modelec_com'
exec: "serial_listener"
name: "serial_listener"
- node:
pkg: 'modelec_com'
exec: "serial_listener"
name: "serial_listener"
- node:
- node:
pkg: 'modelec_com'
exec: 'pcb_odo_interface'
name: 'pcb_odo_interface'
- node:
- node:
pkg: 'modelec_com'
exec: 'pcb_alim_interface'
name: 'pcb_alim_interface'
- node:
- node:
pkg: 'modelec_gui'
exec: "modelec_gui"
name: "modelec_gui"
- node:
- node:
pkg: 'modelec_core'
exec: 'speed_result'
name: 'speed_result'
- node:
- node:
pkg: 'modelec_strat'
exec: 'strat_fsm'
name: 'strat_fsm'
- node:
pkg: 'modelec_strat'
exec: 'pami_manager'
name: 'pami_manager'

21
src/modelec_core/launch/test.no_gui.modelec.launch.yml
View File

@@ -1,26 +1,31 @@
launch:
- node:
pkg: 'modelec_com'
exec: "serial_listener"
name: "serial_listener"
- node:
pkg: 'modelec_com'
exec: "serial_listener"
name: "serial_listener"
- node:
- node:
pkg: 'modelec_com'
exec: 'pcb_odo_interface'
name: 'pcb_odo_interface'
- node:
- node:
pkg: 'modelec_com'
exec: 'pcb_alim_interface'
name: 'pcb_alim_interface'
- node:
- node:
pkg: 'modelec_core'
exec: 'speed_result'
name: 'speed_result'
- node:
- node:
pkg: 'modelec_strat'
exec: 'strat_fsm'
name: 'strat_fsm'
- node:
pkg: 'modelec_strat'
exec: 'pami_manager'
name: 'pami_manager'

21
src/modelec_core/launch/test.no_strat.modelec.launch.yml
View File

@@ -1,26 +1,31 @@
launch:
- node:
pkg: 'modelec_com'
exec: "serial_listener"
name: "serial_listener"
- node:
pkg: 'modelec_com'
exec: "serial_listener"
name: "serial_listener"
- node:
- node:
pkg: 'modelec_com'
exec: 'pcb_odo_interface'
name: 'pcb_odo_interface'
- node:
- node:
pkg: 'modelec_com'
exec: 'pcb_alim_interface'
name: 'pcb_alim_interface'
- node:
- node:
pkg: 'modelec_gui'
exec: "modelec_gui"
name: "modelec_gui"
- node:
- node:
pkg: 'modelec_core'
exec: 'speed_result'
name: 'speed_result'
- node:
pkg: 'modelec_strat'
exec: 'pami_manager'
name: 'pami_manager'

10
src/modelec_strat/CMakeLists.txt
View File

@@ -31,6 +31,15 @@ target_include_directories(enemy_manager PUBLIC
$<INSTALL_INTERFACE:include>
)
add_executable(pami_manager src/pami_manager.cpp src/obstacle/obstacle.cpp)
ament_target_dependencies(pami_manager rclcpp std_msgs std_srvs modelec_interfaces ament_index_cpp)
target_link_libraries(pami_manager modelec_utils::utils modelec_utils::config)
target_include_directories(pami_manager PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
if (BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
set(ament_cmake_copyright_FOUND TRUE)
@@ -41,6 +50,7 @@ endif ()
install(TARGETS
strat_fsm
enemy_manager
pami_manager
DESTINATION lib/${PROJECT_NAME}
)

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

@@ -67,4 +67,8 @@
<all_party>10</all_party>
</pami>
</mission_score>
<pami>
<time_to_put_zone>77</time_to_put_zone>
<time_to_remove_top_pot>65</time_to_remove_top_pot>
</pami>
</config>

3
src/modelec_strat/data/pami_zone.xml Normal file
View File

@@ -0,0 +1,3 @@
<pami_zone>
<middle id="1000" x="1500" y="1500" theta="-1.5708" width="1700" height="600" type="pami-middle" />
</pami_zone>

45
src/modelec_strat/include/modelec_strat/pami_manager.hpp Normal file
View File

@@ -0,0 +1,45 @@
#pragma once
#include <rclcpp/rclcpp.hpp>
#include <modelec_utils/config.hpp>
#include <modelec_utils/point.hpp>
#include <modelec_strat/obstacle/obstacle.hpp>
#include <tinyxml2.h>
#include <std_msgs/msg/int64.hpp>
namespace Modelec {
class PamiManger : public rclcpp::Node
{
public:
PamiManger();
bool ReadFromXML(const std::string& filename);
protected:
Obstacle zone;
int time_to_put_zone_ = 0;
int time_to_remove_top_pot_ = 0;
int score_to_add_ = 0;
int score_goupie_ = 0;
int score_superstar_ = 0;
int score_all_party_ = 0;
int score_free_zone_ = 0;
rclcpp::TimerBase::SharedPtr timer_add_;
rclcpp::TimerBase::SharedPtr timer_remove_;
rclcpp::Subscription<std_msgs::msg::Int64>::SharedPtr start_time_sub_;
rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr add_obs_pub_;
rclcpp::Publisher<modelec_interfaces::msg::Obstacle>::SharedPtr remove_obs_pub_;
rclcpp::Publisher<std_msgs::msg::Int64>::SharedPtr score_pub_;
};
}

114
src/modelec_strat/src/pami_manager.cpp Normal file
View File

@@ -0,0 +1,114 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <modelec_strat/pami_manager.hpp>
namespace Modelec
{
PamiManger::PamiManger() : Node("pami_manager")
{
std::string config_file = ament_index_cpp::get_package_share_directory("modelec_strat") +
"/data/config.xml";
if (!Config::load(config_file))
{
RCLCPP_ERROR(get_logger(), "Failed to load configuration");
}
time_to_put_zone_ = Config::get<int>("config.pami.time_to_put_zone", 80);
time_to_remove_top_pot_ = Config::get<int>("config.pami.time_to_remove_top_pot", 70);
score_goupie_ = Config::get<int>("config.mission_score.pami.goupie");
score_superstar_ = Config::get<int>("config.mission_score.pami.superstar");
score_all_party_ = Config::get<int>("config.mission_score.pami.all_party");
score_free_zone_ = 0;
score_to_add_ = score_goupie_ + score_superstar_ + score_all_party_ + score_free_zone_;
std::string obstacles_path = ament_index_cpp::get_package_share_directory("modelec_strat") +
"/data/pami_zone.xml";
if (!ReadFromXML(obstacles_path))
{
RCLCPP_ERROR(get_logger(), "Failed to load obstacles from XML");
}
start_time_sub_ = create_subscription<std_msgs::msg::Int64>(
"/strat/start_time", 10, [this](const std_msgs::msg::Int64::SharedPtr msg)
{
auto now = std::chrono::system_clock::now().time_since_epoch();
auto goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_remove_top_pot_);
auto second_goal = std::chrono::nanoseconds(msg->data) + std::chrono::seconds(time_to_put_zone_);
timer_remove_ = create_wall_timer(
goal - now,
[this]()
{
modelec_interfaces::msg::Obstacle topLeft;
topLeft.id = 10;
remove_obs_pub_->publish(topLeft);
modelec_interfaces::msg::Obstacle topRight;
topRight.id = 20;
remove_obs_pub_->publish(topRight);
timer_remove_->cancel();
});
timer_add_ = create_wall_timer(
second_goal - now,
[this]()
{
add_obs_pub_->publish(zone.toMsg());
std_msgs::msg::Int64 msg;
msg.data = score_to_add_;
score_pub_->publish(msg);
timer_add_->cancel();
});
});
add_obs_pub_ = create_publisher<modelec_interfaces::msg::Obstacle>(
"obstacle/add", 10);
remove_obs_pub_ = create_publisher<modelec_interfaces::msg::Obstacle>(
"obstacle/remove", 10);
score_pub_ = create_publisher<std_msgs::msg::Int64>("/strat/score", 10);
}
bool PamiManger::ReadFromXML(const std::string& filename)
{
tinyxml2::XMLDocument doc;
if (doc.LoadFile(filename.c_str()) != tinyxml2::XML_SUCCESS)
{
RCLCPP_ERROR(get_logger(), "Failed to load obstacle XML file: %s", filename.c_str());
return false;
}
tinyxml2::XMLElement* root = doc.FirstChildElement("pami_zone");
if (!root)
{
RCLCPP_ERROR(get_logger(), "No <pami-zone> root element in file");
return false;
}
tinyxml2::XMLElement* elem = root->FirstChildElement("middle");
if (!elem)
{
RCLCPP_ERROR(get_logger(), "No <middle> element in file");
return false;
}
zone = Obstacle(elem);
RCLCPP_INFO(get_logger(), "Loaded zone obstacles from XML");
return true;
}
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<Modelec::PamiManger>());
rclcpp::shutdown();
return 0;
}