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

ask waypoint to pcb

Browse Source
This commit is contained in:
acki
2025-12-17 20:11:22 +01:00
parent 9696385a9e
commit c4a8c282fe
7 changed files with 64 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -56,6 +56,7 @@ namespace Modelec
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_;
rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr odo_ask_active_waypoint_subscriber_;
rclcpp::Subscription<sensor_msgs::msg::Joy>::SharedPtr joy_subscriber_;
void AddWaypointCallback(const modelec_interfaces::msg::OdometryWaypoint::SharedPtr msg);

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

@@ -69,6 +69,13 @@ namespace Modelec
SetPIDCallback(msg);
});
odo_ask_active_waypoint_subscriber_ = this->create_subscription<std_msgs::msg::Empty>(
"odometry/ask_active_waypoint", 10,
[this](const std_msgs::msg::Empty::SharedPtr)
{
GetData("WAYPOINT", {"ACTIVE"});
});
joy_subscriber_ = this->create_subscription<sensor_msgs::msg::Joy>(
"joy", 30,
[this](const sensor_msgs::msg::Joy::SharedPtr msg)
@@ -170,14 +177,38 @@ namespace Modelec
}
else if (tokens[1] == "WAYPOINT")
{
RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[2].c_str());
if (tokens[2] == "REACH")
{
RCLCPP_INFO(this->get_logger(), "Waypoint reached: ID %s", tokens[2].c_str());
int id = std::stoi(tokens[2]);
int id = std::stoi(tokens[2]);
auto message = modelec_interfaces::msg::OdometryWaypoint();
message.id = id;
auto message = modelec_interfaces::msg::OdometryWaypoint();
message.id = id;
odo_waypoint_reach_publisher_->publish(message);
odo_waypoint_reach_publisher_->publish(message);
}
else if (tokens.size() >= 7)
{
int id = std::stoi(tokens[2]);
bool is_end = tokens[3] == "1";
long x = std::stol(tokens[4]);
long y = std::stol(tokens[5]);
double theta = std::stod(tokens[6]);
bool reach = tokens.size() > 7 ? tokens[7] == "1" : false;
if (reach)
{
auto message = modelec_interfaces::msg::OdometryWaypoint();
message.id = id;
message.is_end = is_end;
message.x = x;
message.y = y;
message.theta = theta;
odo_waypoint_reach_publisher_->publish(message);
}
}
}
else if (tokens[1] == "PID")
{

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

@@ -113,6 +113,8 @@ namespace Modelec
Point GetSpawn() const;
void AskWaypoint();
protected:
void OnWaypointReach(const WaypointMsg::SharedPtr msg);
@@ -171,8 +173,7 @@ namespace Modelec
rclcpp::Publisher<modelec_interfaces::msg::Spawn>::SharedPtr spawn_pub_;
rclcpp::Service<std_srvs::srv::Empty>::SharedPtr ask_spawn_srv_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr odo_get_pos_pub_;
rclcpp::Time last_odo_get_pos_time_;
rclcpp::Publisher<std_msgs::msg::Empty>::SharedPtr odo_ask_waypoint_pub_;
};

5
src/modelec_strat/src/missions/free_mission.cpp
View File

@@ -24,6 +24,11 @@ namespace Modelec {
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 2)
{
nav_->AskWaypoint();
return;
}
if ((node_->now() - go_timeout_).seconds() < 10)
{
return;

5
src/modelec_strat/src/missions/go_home_mission.cpp
View File

@@ -27,6 +27,11 @@ namespace Modelec
{
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 2)
{
nav_->AskWaypoint();
return;
}
if ((node_->now() - go_timeout_).seconds() < 10)
{
return;

7
src/modelec_strat/src/missions/take_mission.cpp
View File

@@ -24,7 +24,12 @@ namespace Modelec {
if (!nav_->HasArrived())
{
if ((node_->now() - go_timeout_).seconds() < 20)
if ((node_->now() - go_timeout_).seconds() < 2)
{
nav_->AskWaypoint();
return;
}
if ((node_->now() - go_timeout_).seconds() < 10)
{
return;
}

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

@@ -97,10 +97,8 @@ namespace Modelec
}
});
odo_get_pos_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
"odometry/get/pos", 30);
last_odo_get_pos_time_ = node_->now();
odo_ask_waypoint_pub_ = node_->create_publisher<std_msgs::msg::Empty>(
"odometry/ask_active_waypoint", 30);
}
void NavigationHelper::ReInit()
@@ -724,6 +722,12 @@ namespace Modelec
return spawn_;
}
void NavigationHelper::AskWaypoint()
{
std_msgs::msg::Empty msg;
odo_ask_waypoint_pub_->publish(msg);
}
void NavigationHelper::OnWaypointReach(const WaypointMsg::SharedPtr msg)
{
for (auto& waypoint : waypoints_)