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

init

Browse Source
This commit is contained in:
acki
2025-05-28 23:25:26 -04:00
parent 3ee9855253
commit 79e687c46d
4 changed files with 24 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -3,6 +3,7 @@
#include <rclcpp/rclcpp.hpp>
#include <std_msgs/msg/string.hpp>
#include <std_msgs/msg/empty.hpp>
#include <std_msgs/msg/bool.hpp>
#include <modelec_interfaces/srv/add_serial_listener.hpp>
#include <modelec_interfaces/msg/action_asc_pos.hpp>
#include <modelec_interfaces/msg/action_relay_state.hpp>
@@ -64,6 +65,7 @@ namespace Modelec
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_start_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_disarm_sub_;
rclcpp::Subscription<std_msgs::msg::Bool>::SharedPtr tir_arm_set_sub_;
bool isOk = false;

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

@@ -186,6 +186,13 @@ namespace Modelec
RespondEvent("TIR", {"DIS"});
});
tir_arm_set_sub_ = this->create_subscription<std_msgs::msg::Bool>(
"action/tir/arm/set", 10,
[this](const std_msgs::msg::Bool::SharedPtr msg)
{
SendOrder("TIR", {"ARM", msg->data ? "1" : "0"});
});
// TODO : check for real value there
asc_value_mapper_ = {
@@ -194,14 +201,14 @@ namespace Modelec
{2, 200},
{3, 300}
};
for (auto & [id, v] : asc_value_mapper_)
/*for (auto & [id, v] : asc_value_mapper_)
{
SendOrder("ASC", {std::to_string(id), std::to_string(v)});
}
}*/
asc_state_ = 3;
SendMove("ASC", {std::to_string(asc_state_)});
// SendMove("ASC", {std::to_string(asc_state_)});
servo_pos_mapper_ = {
{0, {{0, M_PI_2}}},
@@ -244,8 +251,6 @@ namespace Modelec
{
SendMove("RELAY" + std::to_string(id), {std::to_string(v)});
}
SendOrder("TIR", {"ARM", "1"});
}
PCBActionInterface::~PCBActionInterface()

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

@@ -74,6 +74,7 @@ namespace Modelec
rclcpp::Subscription<modelec_interfaces::msg::Spawn>::SharedPtr spawn_id_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr reset_strat_sub_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr tir_arm_sub_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr tir_arm_set_pub_;
rclcpp::Publisher<std_msgs::msg::Bool>::SharedPtr start_odo_pub_;
};

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

@@ -30,6 +30,14 @@ namespace Modelec
spawn_id_sub_ = create_subscription<modelec_interfaces::msg::Spawn>(
"/strat/spawn", 10, [this](const modelec_interfaces::msg::Spawn::SharedPtr msg)
{
std_msgs::msg::Bool start_odo_msg;
start_odo_msg.data = true;
start_odo_pub_->publish(start_odo_msg);
std_msgs::msg::Bool tir_msg;
tir_msg.data = true;
tir_arm_set_pub_->publish(tir_msg);
team_selected_ = true;
team_id_ = msg->team_id;
nav_->SetTeamId(team_id_);
@@ -48,6 +56,9 @@ namespace Modelec
setup_ = true;
});
tir_arm_set_pub_ = create_publisher<std_msgs::msg::Bool>(
"/action/tir/arm/set", 10);
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";
@@ -118,10 +129,6 @@ namespace Modelec
case State::INIT:
if (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;