diff --git a/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h b/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h index e10bee8..2ede212 100644 --- a/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h +++ b/Controllers/Packages/amr_control/include/amr_control/amr_publiser.h @@ -3,8 +3,17 @@ #include #include -#include + #include +#include +#include +#include +#include +#include + +#include +#include +#include #include #include #include @@ -14,12 +23,13 @@ namespace amr_control { - struct CostmapObject + struct PlannerObject { // ROS publishers ros::Publisher pub_; ros::Publisher update_pub_; ros::Publisher footprint_pub_; + ros::Publisher plan_pub_; ros::Timer timer_; std::string topic_; bool active_; @@ -47,6 +57,16 @@ namespace amr_control */ virtual ~AmrPublisher(); + /** + * @brief Publish global plan to RViz + */ + void publishGlobalPlan(); + + /** + * @brief Publish local plan to RViz + */ + void publishLocalPlan(); + /** * @brief Publish global costmap to RViz * @param publish_rate Rate at which to publish (Hz). If 0, publish immediately once. @@ -96,7 +116,7 @@ namespace amr_control * @brief Check if publishing is active * @return True if publishing is active (either global or local) */ - bool isPublishing() const { return global_costmap_obj_.active_ || local_costmap_obj_.active_ || cmd_vel_publishing_active_; } + bool isPublishing() const { return global_planner_obj_.active_ || local_planner_obj_.active_ || cmd_vel_publishing_active_; } /** * @brief Start publishing cmd_vel at a fixed rate using ros::WallTimer @@ -109,7 +129,28 @@ namespace amr_control */ void stopCmdVelPublishing(); + /** + * @brief Start publishing plans at a fixed rate + * @param global_rate Publishing rate for global plan (Hz). If 0, don't publish global. + * @param local_rate Publishing rate for local plan (Hz). If 0, don't publish local. + */ + void startPlanPublishing(double global_rate = 10.0, double local_rate = 10.0); + + /** + * @brief Stop publishing plans + */ + void stopPlanPublishing(); + private: + + /** + * @brief Convert robot_nav_2d_msgs::Path2D to nav_msgs::Path + * @param robot_path Input robot_nav_2d_msgs::Path2D + * @param nav_path Output nav_msgs::Path + */ + void convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, + nav_msgs::Path &nav_path); + /** * @brief Callback for new subscription to global costmap * @param pub SingleSubscriberPublisher for the new subscription @@ -164,16 +205,38 @@ namespace amr_control */ void cmdVelTimerCallback(const ros::WallTimerEvent &event); + /** + * @brief Timer callback for updating global plan + * @param event Timer event + */ + void globalPlanTimerCallback(const ros::TimerEvent &event); + + /** + * @brief Timer callback for updating local plan + * @param event Timer event + */ + void localPlanTimerCallback(const ros::TimerEvent &event); + ros::NodeHandle nh_; robot::move_base_core::BaseNavigation::Ptr move_base_ptr_; - CostmapObject global_costmap_obj_; - CostmapObject local_costmap_obj_; + PlannerObject global_planner_obj_; + PlannerObject local_planner_obj_; ros::Publisher cmd_vel_pub_; // ros::WallTimer for cmd_vel publishing ros::WallTimer cmd_vel_timer_; bool cmd_vel_publishing_active_; + + // Timers for plan publishing + ros::Timer global_plan_timer_; + ros::Timer local_plan_timer_; + bool global_plan_publishing_active_; + bool local_plan_publishing_active_; + + // Cached plans with timestamps + robot_nav_2d_msgs::Path2D cached_global_plan_; + robot_nav_2d_msgs::Path2D cached_local_plan_; }; } // namespace amr_control diff --git a/Controllers/Packages/amr_control/src/amr_control.cpp b/Controllers/Packages/amr_control/src/amr_control.cpp index 65042e5..e7c8796 100644 --- a/Controllers/Packages/amr_control/src/amr_control.cpp +++ b/Controllers/Packages/amr_control/src/amr_control.cpp @@ -77,11 +77,12 @@ namespace amr_control vda_5050_client_api_ptr_.reset(); } - // Stop costmap and cmd_vel publishing + // Stop costmap, cmd_vel and plan publishing if (amr_publisher_ptr_) { amr_publisher_ptr_->stopPublishing(); amr_publisher_ptr_->stopCmdVelPublishing(); + amr_publisher_ptr_->stopPlanPublishing(); amr_publisher_ptr_.reset(); } } @@ -339,6 +340,15 @@ namespace amr_control nh.param("cmd_vel_publish_rate", cmd_vel_rate, cmd_vel_rate); robot::log_info("[%s:%d]\n Starting cmd_vel publishing at %.2f Hz...", __FILE__, __LINE__, cmd_vel_rate); amr_publisher_ptr_->startCmdVelPublishing(cmd_vel_rate); + + // Start publishing plans using Timer + double global_plan_rate = 10.0; // Default: 10 Hz + double local_plan_rate = 10.0; // Default: 10 Hz + nh.param("global_plan_publish_rate", global_plan_rate, global_plan_rate); + nh.param("local_plan_publish_rate", local_plan_rate, local_plan_rate); + robot::log_info("[%s:%d]\n Starting plan publishing - Global: %.2f Hz, Local: %.2f Hz...", + __FILE__, __LINE__, global_plan_rate, local_plan_rate); + amr_publisher_ptr_->startPlanPublishing(global_plan_rate, local_plan_rate); } } } diff --git a/Controllers/Packages/amr_control/src/amr_publiser.cpp b/Controllers/Packages/amr_control/src/amr_publiser.cpp index 9e83fd2..7d5bf54 100644 --- a/Controllers/Packages/amr_control/src/amr_publiser.cpp +++ b/Controllers/Packages/amr_control/src/amr_publiser.cpp @@ -3,8 +3,7 @@ #include #include #include -#include -#include + namespace amr_control { @@ -22,29 +21,34 @@ AmrPublisher::AmrPublisher(ros::NodeHandle &nh, robot::move_base_core::BaseNavig nh_.param("local_costmap_topic", local_costmap_topic, local_costmap_topic); nh_.param("cmd_vel_topic", cmd_vel_topic, cmd_vel_topic); // Initialize publishers with callback for new subscriptions - global_costmap_obj_.pub_ = nh_.advertise( + global_planner_obj_.pub_ = nh_.advertise( global_costmap_topic, 1, boost::bind(&AmrPublisher::onNewSubscriptionGlobal, this, _1)); - global_costmap_obj_.update_pub_ = nh_.advertise(global_costmap_topic + "_updates", 1); - global_costmap_obj_.footprint_pub_ = nh_.advertise(global_costmap_topic + "/footprint", 1); + global_planner_obj_.update_pub_ = nh_.advertise(global_costmap_topic + "_updates", 1); + global_planner_obj_.footprint_pub_ = nh_.advertise(global_costmap_topic + "/footprint", 1); + global_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(global_costmap_topic + "/plan", 1); - local_costmap_obj_.pub_ = nh_.advertise( + local_planner_obj_.pub_ = nh_.advertise( local_costmap_topic, 1, boost::bind(&AmrPublisher::onNewSubscriptionLocal, this, _1)); - local_costmap_obj_.update_pub_ = nh_.advertise(local_costmap_topic + "_updates", 1); - local_costmap_obj_.footprint_pub_ = nh_.advertise(local_costmap_topic + "/footprint", 1); - + local_planner_obj_.update_pub_ = nh_.advertise(local_costmap_topic + "_updates", 1); + local_planner_obj_.footprint_pub_ = nh_.advertise(local_costmap_topic + "/footprint", 1); + local_planner_obj_.plan_pub_ = nh_.advertise< nav_msgs::Path>(local_costmap_topic + "/plan", 1); cmd_vel_pub_ = nh_.advertise(cmd_vel_topic, 1); cmd_vel_publishing_active_ = false; robot::log_info("[AmrPublisher] Initialized. Cmd vel topic: %s", cmd_vel_topic.c_str()); // Initialize topic names in CostmapObject - global_costmap_obj_.topic_ = global_costmap_topic; - local_costmap_obj_.topic_ = local_costmap_topic; + global_planner_obj_.topic_ = global_costmap_topic; + local_planner_obj_.topic_ = local_costmap_topic; // Initialize other CostmapObject members - global_costmap_obj_.active_ = false; - local_costmap_obj_.active_ = false; + global_planner_obj_.active_ = false; + local_planner_obj_.active_ = false; + + // Initialize plan publishing flags + global_plan_publishing_active_ = false; + local_plan_publishing_active_ = false; robot::log_info("[AmrPublisher] Initialized. Global costmap topic: %s, Local costmap topic: %s", @@ -58,6 +62,7 @@ AmrPublisher::~AmrPublisher() { stopPublishing(); stopCmdVelPublishing(); + stopPlanPublishing(); } void AmrPublisher::convertToNavOccupancyGrid(const robot_nav_msgs::OccupancyGrid &robot_grid, @@ -138,13 +143,13 @@ void AmrPublisher::publishGlobalCostmap(double publish_rate) { nav_msgs::OccupancyGrid nav_grid; convertToNavOccupancyGrid(global_data.costmap, nav_grid); - global_costmap_obj_.pub_.publish(nav_grid); + global_planner_obj_.pub_.publish(nav_grid); } else { map_msgs::OccupancyGridUpdate grid_update; convertToRobotOccupancyGridUpdate(global_data.costmap_update, grid_update); - global_costmap_obj_.update_pub_.publish(grid_update); + global_planner_obj_.update_pub_.publish(grid_update); } // Publish footprint @@ -177,13 +182,13 @@ void AmrPublisher::publishLocalCostmap(double publish_rate) { nav_msgs::OccupancyGrid nav_grid; convertToNavOccupancyGrid(local_data.costmap, nav_grid); - local_costmap_obj_.pub_.publish(nav_grid); + local_planner_obj_.pub_.publish(nav_grid); } else { map_msgs::OccupancyGridUpdate grid_update; convertToRobotOccupancyGridUpdate(local_data.costmap_update, grid_update); - local_costmap_obj_.update_pub_.publish(grid_update); + local_planner_obj_.update_pub_.publish(grid_update); } // Publish footprint @@ -220,7 +225,7 @@ void AmrPublisher::publishGlobalFootprint() // Convert and publish footprint geometry_msgs::PolygonStamped nav_footprint; convertToNavPolygonStamped(global_data.footprint, nav_footprint); - global_costmap_obj_.footprint_pub_.publish(nav_footprint); + global_planner_obj_.footprint_pub_.publish(nav_footprint); // robot::log_debug("[AmrPublisher] Published global footprint with %zu points, frame_id='%s'", // nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str()); @@ -247,7 +252,7 @@ void AmrPublisher::publishLocalFootprint() // Convert and publish footprint geometry_msgs::PolygonStamped nav_footprint; convertToNavPolygonStamped(local_data.footprint, nav_footprint); - local_costmap_obj_.footprint_pub_.publish(nav_footprint); + local_planner_obj_.footprint_pub_.publish(nav_footprint); // robot::log_debug("[AmrPublisher] Published local footprint with %zu points, frame_id='%s'", // nav_footprint.polygon.points.size(), nav_footprint.header.frame_id.c_str()); @@ -271,8 +276,8 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate) // Start global costmap timer if rate > 0 if (global_rate > 0.0) { - global_costmap_obj_.active_ = true; - global_costmap_obj_.timer_ = nh_.createTimer( + global_planner_obj_.active_ = true; + global_planner_obj_.timer_ = nh_.createTimer( ros::Duration(1.0 / global_rate), &AmrPublisher::globalCostmapTimerCallback, this @@ -283,8 +288,8 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate) // Start local costmap timer if rate > 0 if (local_rate > 0.0) { - local_costmap_obj_.active_ = true; - local_costmap_obj_.timer_ = nh_.createTimer( + local_planner_obj_.active_ = true; + local_planner_obj_.timer_ = nh_.createTimer( ros::Duration(1.0 / local_rate), &AmrPublisher::localCostmapTimerCallback, this @@ -296,16 +301,16 @@ void AmrPublisher::startPublishing(double global_rate, double local_rate) void AmrPublisher::stopPublishing() { // Stop timers - if (global_costmap_obj_.active_) + if (global_planner_obj_.active_) { - global_costmap_obj_.timer_.stop(); - global_costmap_obj_.active_ = false; + global_planner_obj_.timer_.stop(); + global_planner_obj_.active_ = false; } - if (local_costmap_obj_.active_) + if (local_planner_obj_.active_) { - local_costmap_obj_.timer_.stop(); - local_costmap_obj_.active_ = false; + local_planner_obj_.timer_.stop(); + local_planner_obj_.active_ = false; } robot::log_info("[AmrPublisher] Stopped publishing costmaps"); @@ -313,7 +318,7 @@ void AmrPublisher::stopPublishing() void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event) { - if (global_costmap_obj_.active_) + if (global_planner_obj_.active_) { publishGlobalCostmap(); } @@ -321,7 +326,7 @@ void AmrPublisher::globalCostmapTimerCallback(const ros::TimerEvent &event) void AmrPublisher::localCostmapTimerCallback(const ros::TimerEvent &event) { - if (local_costmap_obj_.active_) + if (local_planner_obj_.active_) { publishLocalCostmap(); } @@ -333,6 +338,61 @@ void AmrPublisher::onNewSubscriptionLocal(const ros::SingleSubscriberPublisher& publishLocalCostmap(); } +void AmrPublisher::convertToNavPath(const robot_nav_2d_msgs::Path2D &robot_path, + nav_msgs::Path &nav_path) +{ + // Convert header + nav_2d_msgs::Path2D path2d; + path2d.header.seq = robot_path.header.seq; + path2d.header.frame_id = robot_path.header.frame_id; + path2d.header.stamp = ros::Time::now(); + path2d.poses.resize(robot_path.poses.size()); + for (unsigned int i = 0; i < robot_path.poses.size(); i++) + { + path2d.poses[i].pose.x = robot_path.poses[i].pose.x; + path2d.poses[i].pose.y = robot_path.poses[i].pose.y; + path2d.poses[i].pose.theta = robot_path.poses[i].pose.theta; + } + nav_path = nav_2d_utils::pathToPath(path2d); +} + +void AmrPublisher::publishGlobalPlan() +{ + if(!move_base_ptr_) + { + return; + } + + // Check if plan is empty + if(cached_global_plan_.poses.empty()) + { + return; + } + + robot::Time now = robot::Time::now(); + robot::Time plan_stamp = cached_global_plan_.header.stamp; + + // Check if timestamp is zero (uninitialized) + if(plan_stamp.sec == 0 && plan_stamp.nsec == 0) + { + return; + } + + robot::Duration age = now - plan_stamp; + robot::Duration max_age(0.5); + + // Check if timestamp is not older than 0.5 seconds + if(plan_stamp < now - max_age) + { + return; + } + + nav_msgs::Path nav_path; + convertToNavPath(cached_global_plan_, nav_path); + + global_planner_obj_.plan_pub_.publish(nav_path); +} + void AmrPublisher::onNewSubscriptionGlobal(const ros::SingleSubscriberPublisher& pub) { robot::log_info("[AmrPublisher] New subscription: %s", pub.getSubscriberName().c_str()); @@ -405,5 +465,128 @@ void AmrPublisher::cmdVelTimerCallback(const ros::WallTimerEvent &event) } } +void AmrPublisher::publishLocalPlan() +{ + if(!move_base_ptr_) + { + return; + } + + // Check if plan is empty + if(cached_local_plan_.poses.empty()) + { + return; + } + + robot::Time now = robot::Time::now(); + robot::Time plan_stamp = cached_local_plan_.header.stamp; + + // Check if timestamp is zero (uninitialized) + if(plan_stamp.sec == 0 && plan_stamp.nsec == 0) + { + return; + } + + robot::Duration age = now - plan_stamp; + robot::Duration max_age(0.5); + + // Check if timestamp is not older than 0.5 seconds + if(plan_stamp < now - max_age) + { + return; + } + + nav_msgs::Path nav_path; + convertToNavPath(cached_local_plan_, nav_path); + local_planner_obj_.plan_pub_.publish(nav_path); +} + +void AmrPublisher::globalPlanTimerCallback(const ros::TimerEvent &event) +{ + if (!move_base_ptr_) + { + return; + } + + // Update cached global plan + try + { + cached_global_plan_ = move_base_ptr_->getGlobalData().plan; + // Publish if timestamp is valid + publishGlobalPlan(); + } + catch (const std::exception &e) + { + robot::log_error("[AmrPublisher] Error updating global plan: %s", e.what()); + } +} + +void AmrPublisher::localPlanTimerCallback(const ros::TimerEvent &event) +{ + if (!move_base_ptr_) + { + return; + } + + // Update cached local plan + try + { + cached_local_plan_ = move_base_ptr_->getLocalData().plan; + // Publish if timestamp is valid + publishLocalPlan(); + } + catch (const std::exception &e) + { + robot::log_error("[AmrPublisher] Error updating local plan: %s", e.what()); + } +} + +void AmrPublisher::startPlanPublishing(double global_rate, double local_rate) +{ + stopPlanPublishing(); // Stop any existing publishing first + + // Start global plan timer if rate > 0 + if (global_rate > 0.0) + { + global_plan_publishing_active_ = true; + global_plan_timer_ = nh_.createTimer( + ros::Duration(1.0 / global_rate), + &AmrPublisher::globalPlanTimerCallback, + this + ); + robot::log_info("[AmrPublisher] Started publishing global plan at %.2f Hz", global_rate); + } + + // Start local plan timer if rate > 0 + if (local_rate > 0.0) + { + local_plan_publishing_active_ = true; + local_plan_timer_ = nh_.createTimer( + ros::Duration(1.0 / local_rate), + &AmrPublisher::localPlanTimerCallback, + this + ); + robot::log_info("[AmrPublisher] Started publishing local plan at %.2f Hz", local_rate); + } +} + +void AmrPublisher::stopPlanPublishing() +{ + // Stop timers + if (global_plan_publishing_active_) + { + global_plan_timer_.stop(); + global_plan_publishing_active_ = false; + } + + if (local_plan_publishing_active_) + { + local_plan_timer_.stop(); + local_plan_publishing_active_ = false; + } + + robot::log_info("[AmrPublisher] Stopped publishing plans"); +} + } // namespace amr_control diff --git a/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp index 59eff2e..b113e3c 100644 --- a/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp +++ b/Controllers/Packages/amr_control/src/amr_vda_5050_client_api.cpp @@ -160,88 +160,85 @@ namespace amr_control } else if (global_plan_msg_type_ == std::string("vda5050_msgs::Order")) { - // vda5050_msgs::Order order_msg; - // order_msg.headerId = order.headerId; - // order_msg.timestamp = order.timestamp; - // order_msg.version = order.version; - // order_msg.manufacturer = order.manufacturer; - // order_msg.serialNumber = order.serialNumber; - // order_msg.orderId = order.orderId; - // order_msg.orderUpdateId = order.orderUpdateId; - - // for (auto node : order.nodes) - // { - // vda5050_msgs::Node node_msg; - // node_msg.nodeId = node.nodeId; - // node_msg.sequenceId = node.sequenceId; - // node_msg.nodeDescription = node.nodeDescription; - // node_msg.released = node.released; - - // node_msg.nodePosition.x = node.nodePosition.x; - // node_msg.nodePosition.y = node.nodePosition.y; - // node_msg.nodePosition.theta = node.nodePosition.theta; - // node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY; - // node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta; - // node_msg.nodePosition.mapId = node.nodePosition.mapId; - // node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription; - // // vda5050_msgs/Action[] actions - // order_msg.nodes.push_back(node_msg); - // } - - // for (auto edge : order.edges) - // { - // vda5050_msgs::Edge edge_msg; - // edge_msg.edgeId = edge.edgeId; - // edge_msg.sequenceId = edge.sequenceId; - // edge_msg.edgeDescription = edge.edgeDescription; - // edge_msg.released = edge.released; - // edge_msg.startNodeId = edge.startNodeId; - // edge_msg.endNodeId = edge.endNodeId; - // edge_msg.maxSpeed = edge.maxSpeed; - // edge_msg.maxHeight = edge.maxHeight; - // edge_msg.minHeight = edge.minHeight; - // edge_msg.orientation = edge.orientation; - // edge_msg.orientationType = edge.orientationType; - // edge_msg.direction = edge.direction; - // edge_msg.rotationAllowed = edge.rotationAllowed; - // edge_msg.maxRotationSpeed = edge.maxRotationSpeed; - // edge_msg.length = edge.length; - // edge_msg.trajectory.degree = edge.trajectory.degree; - // edge_msg.trajectory.knotVector = edge.trajectory.knotVector; - - // for (auto controlPoint : edge.trajectory.controlPoints) - // { - // vda5050_msgs::ControlPoint controlPoint_msg; - // controlPoint_msg.x = controlPoint.x; - // controlPoint_msg.y = controlPoint.y; - // controlPoint_msg.weight = controlPoint.weight; - // edge_msg.trajectory.controlPoints.push_back(controlPoint_msg); - // } - // // Corridor corridor; - // // std::vector actions; - // order_msg.edges.push_back(edge_msg); - // } - - // // order_msg.edges zoneSetId =VDA5050ClientAPI::client_ptr_->vda5050_order_. - // VDA5050ClientAPI::plan_pub_.publish(order_msg); - - // vda_5050::NodePosition position = order.nodes.back().nodePosition; - // goal.header.stamp = robot::Time::now(); - // goal.header.frame_id = "map"; - // goal.pose.position.x = position.x; - // goal.pose.position.y = position.y; - // goal.pose.position.z = 0.0; - - // // Convert theta (yaw) to Quaternion using setRPY - // // setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used - // tf3::Quaternion quat; - // quat.setRPY(0.0, 0.0, position.theta); - // goal.pose.orientation.x = quat.x(); - // goal.pose.orientation.y = quat.y(); - // goal.pose.orientation.z = quat.z(); - // goal.pose.orientation.w = quat.w(); - robot_protocol_msgs::Order order_msg; + // vda5050_msgs::Order order_msg; + order_msg.headerId = order.headerId; + order_msg.timestamp = order.timestamp; + order_msg.version = order.version; + order_msg.manufacturer = order.manufacturer; + order_msg.serialNumber = order.serialNumber; + order_msg.orderId = order.orderId; + order_msg.orderUpdateId = order.orderUpdateId; + + for (auto node : order.nodes) + { + robot_protocol_msgs::Node node_msg; + node_msg.nodeId = node.nodeId; + node_msg.sequenceId = node.sequenceId; + node_msg.nodeDescription = node.nodeDescription; + node_msg.released = node.released; + + node_msg.nodePosition.x = node.nodePosition.x; + node_msg.nodePosition.y = node.nodePosition.y; + node_msg.nodePosition.theta = node.nodePosition.theta; + node_msg.nodePosition.allowedDeviationXY = node.nodePosition.allowedDeviationXY; + node_msg.nodePosition.allowedDeviationTheta = node.nodePosition.allowedDeviationTheta; + node_msg.nodePosition.mapId = node.nodePosition.mapId; + node_msg.nodePosition.mapDescription = node.nodePosition.mapDescription; + // vda5050_msgs/Action[] actions + order_msg.nodes.push_back(node_msg); + } + + for (auto edge : order.edges) + { + robot_protocol_msgs::Edge edge_msg; + edge_msg.edgeId = edge.edgeId; + edge_msg.sequenceId = edge.sequenceId; + edge_msg.edgeDescription = edge.edgeDescription; + edge_msg.released = edge.released; + edge_msg.startNodeId = edge.startNodeId; + edge_msg.endNodeId = edge.endNodeId; + edge_msg.maxSpeed = edge.maxSpeed; + edge_msg.maxHeight = edge.maxHeight; + edge_msg.minHeight = edge.minHeight; + edge_msg.orientation = edge.orientation; + edge_msg.orientationType = edge.orientationType; + edge_msg.direction = edge.direction; + edge_msg.rotationAllowed = edge.rotationAllowed; + edge_msg.maxRotationSpeed = edge.maxRotationSpeed; + edge_msg.length = edge.length; + edge_msg.trajectory.degree = edge.trajectory.degree; + edge_msg.trajectory.knotVector = edge.trajectory.knotVector; + + for (auto controlPoint : edge.trajectory.controlPoints) + { + robot_protocol_msgs::ControlPoint controlPoint_msg; + controlPoint_msg.x = controlPoint.x; + controlPoint_msg.y = controlPoint.y; + controlPoint_msg.weight = controlPoint.weight; + edge_msg.trajectory.controlPoints.push_back(controlPoint_msg); + } + // Corridor corridor; + // std::vector actions; + order_msg.edges.push_back(edge_msg); + } + + vda_5050::NodePosition position = order.nodes.back().nodePosition; + goal.header.stamp = robot::Time::now(); + goal.header.frame_id = "map"; + goal.pose.position.x = position.x; + goal.pose.position.y = position.y; + goal.pose.position.z = 0.0; + + // Convert theta (yaw) to Quaternion using setRPY + // setRPY(roll, pitch, yaw) - for 2D navigation, only yaw is used + tf3::Quaternion quat; + quat.setRPY(0.0, 0.0, position.theta); + goal.pose.orientation.x = quat.x(); + goal.pose.orientation.y = quat.y(); + goal.pose.orientation.z = quat.z(); + goal.pose.orientation.w = quat.w(); + VDA5050ClientAPI::move_base_ptr_->moveTo(order_msg, goal); } else @@ -260,7 +257,6 @@ namespace amr_control robot_geometry_msgs::Vector3 angular; angular.z = cmd_vel_max_.theta; VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular); - } } @@ -397,8 +393,6 @@ namespace amr_control robot_geometry_msgs::Vector3 angular; angular.z = cmd_vel_max_.theta; VDA5050ClientAPI::move_base_ptr_->setTwistAngular(angular); - - } } diff --git a/Controllers/Packages/amr_startup/rviz/navigation.rviz b/Controllers/Packages/amr_startup/rviz/navigation.rviz index d925eba..986e7a0 100644 --- a/Controllers/Packages/amr_startup/rviz/navigation.rviz +++ b/Controllers/Packages/amr_startup/rviz/navigation.rviz @@ -7,6 +7,8 @@ Panels: - /Global Options1 - /Grid1/Offset1 - /TF1/Frames1 + - /Global Map1 + - /Local Map1 Splitter Ratio: 0.37295082211494446 Tree Height: 682 - Class: rviz/Selection @@ -363,13 +365,13 @@ Visualization Manager: Buffer Length: 1 Class: rviz/Path Color: 25; 255; 0 - Enabled: false + Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.20000000298023224 Line Style: Lines Line Width: 0.05000000074505806 - Name: Full Plan + Name: Plan Offset: X: 0 Y: 0 @@ -380,9 +382,9 @@ Visualization Manager: Radius: 0.019999999552965164 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 - Topic: /amr_node/SBPLLatticePlanner/plan + Topic: /global_costmap/costmap/plan Unreliable: false - Value: false + Value: true - Alpha: 1 Class: rviz/Polygon Color: 255; 0; 0 @@ -392,82 +394,10 @@ Visualization Manager: Topic: /global_costmap/costmap/footprint Unreliable: false Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 25; 255; 0 - Enabled: true - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.00800000037997961 - Line Style: Billboards - Line Width: 0.009999999776482582 - Name: CustomPath - Offset: - X: 0 - Y: 0 - Z: 0.8999999761581421 - Pose Color: 255; 85; 255 - Pose Style: None - Queue Size: 10 - Radius: 0.004999999888241291 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /amr_node/CustomPlanner/plan - Unreliable: false - Value: true - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 0; 0; 0 - Enabled: false - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.30000001192092896 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Straight Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: Axes - Queue Size: 10 - Radius: 0.029999999329447746 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /amr_node/LocalPlannerAdapter/MKTGoStraightLocalPlanner/global_plan - Unreliable: false - Value: false Enabled: true Name: Global Map - Class: rviz/Group Displays: - - Alpha: 1 - Buffer Length: 1 - Class: rviz/Path - Color: 252; 175; 62 - Enabled: false - Head Diameter: 0.30000001192092896 - Head Length: 0.20000000298023224 - Length: 0.03999999910593033 - Line Style: Lines - Line Width: 0.029999999329447746 - Name: Global Path - Offset: - X: 0 - Y: 0 - Z: 0 - Pose Color: 255; 85; 255 - Pose Style: Axes - Queue Size: 10 - Radius: 0.009999999776482582 - Shaft Diameter: 0.10000000149011612 - Shaft Length: 0.10000000149011612 - Topic: /amr_node/LocalPlannerAdapter/global_plan - Unreliable: false - Value: false - Alpha: 0.699999988079071 Class: rviz/Map Color Scheme: map @@ -491,13 +421,13 @@ Visualization Manager: Buffer Length: 1 Class: rviz/Path Color: 138; 226; 52 - Enabled: false + Enabled: true Head Diameter: 0.30000001192092896 Head Length: 0.20000000298023224 Length: 0.029999999329447746 Line Style: Billboards Line Width: 0.029999999329447746 - Name: Local Plan + Name: Plan Offset: X: 0 Y: 0 @@ -508,37 +438,9 @@ Visualization Manager: Radius: 0.009999999776482582 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 - Topic: /amr_node/LocalPlannerAdapter/transformed_global_plan + Topic: /local_costmap/costmap/plan Unreliable: false - Value: false - - Alpha: 1 - Autocompute Intensity Bounds: true - Autocompute Value Bounds: - Max Value: 10 - Min Value: -10 - Value: true - Axis: Z - Channel Name: intensity - Class: rviz/PointCloud2 - Color: 255; 255; 255 - Color Transformer: Intensity - Decay Time: 0 - Enabled: false - Invert Rainbow: false - Max Color: 255; 255; 255 - Min Color: 0; 0; 0 - Name: PointCloud2 - Position Transformer: XYZ - Queue Size: 10 - Selectable: true - Size (Pixels): 3 - Size (m): 0.009999999776482582 - Style: Flat Squares - Topic: /amr_node/PredictiveTrajectory/cost_cloud - Unreliable: false - Use Fixed Frame: true - Use rainbow: true - Value: false + Value: true - Class: rviz/Group Displays: - Alpha: 1 @@ -702,10 +604,10 @@ Visualization Manager: Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: -62.776824951171875 + Scale: -105.0660171508789 Target Frame: base_link - X: 1.189023494720459 - Y: 0.44669991731643677 + X: 2.555877447128296 + Y: 1.0760011672973633 Saved: - Angle: -34.55989074707031 Class: rviz/TopDownOrtho @@ -727,7 +629,7 @@ Window Geometry: Height: 833 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd000000040000000000000156000002e7fc0200000008fb000000100044006900730070006c006100790073010000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d00650100000000000004500000000000000000000002d9000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002e7fc0200000008fb000000100044006900730070006c006100790073000000003d000002e7000000c900fffffffb0000001200530065006c0065006300740069006f006e000000003d000002590000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001b500000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000000a0049006d006100670065000000003d0000039e0000000000000000fb0000000c00430061006d006500720061020000007c0000013600000280000001e0fb0000000c00430061006d0065007200610200000644000000b7000002c1000002f9000000010000010f00000259fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000259000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b20000000000000000000000020000041800000168fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007520000003efc0100000002fb0000000800540069006d00650000000000000007520000041800fffffffb0000000800540069006d0065010000000000000450000000000000000000000435000002e700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/pnkx_nav_core b/pnkx_nav_core index 57b77ac..0f58db3 160000 --- a/pnkx_nav_core +++ b/pnkx_nav_core @@ -1 +1 @@ -Subproject commit 57b77ac14b144acddccc95cc9dd7e8d4d58728dc +Subproject commit 0f58db34810790067177408f37dc0becb91620c1