add ugv rviz optimal list

Author: ZL199809

Modules/communication/include/rviz_reduce_the_frequency.hpp

@@ -41,6 +41,8 @@ class ReduceTheFrequency
 
     void optimalListCb(const visualization_msgs::Marker::ConstPtr &msg);
 
+    void ugvOptimalListCb(const visualization_msgs::Marker::ConstPtr &msg);
+
     void ugvPointCloudCb(const sensor_msgs::PointCloud2::ConstPtr &msg);
     void ugvOdomCb(const nav_msgs::Odometry::ConstPtr &msg);
     void ugvPathCb(const nav_msgs::Path::ConstPtr &msg);
@@ -60,7 +62,7 @@ class ReduceTheFrequency
 private:
     ros::Subscriber octomap_point_cloud_centers_sub_,occupancy_inflate_sub_,scan_sub_,
         scan_filtered_sub_,trajectory_sub_,odom_sub_,tf_sub_,uav_mesh_sub_,optimal_list_sub_,
-        ugv_point_cloud_sub_,ugv_odom_sub_,ugv_path_sub_;
+        ugv_point_cloud_sub_,ugv_odom_sub_,ugv_path_sub_,ugv_optimal_list_sub_;
 
     // 1000ms
     ros::Publisher octomap_pub_,occupancy_inflate_pub_,scan_pub_,scan_filtered_pub_,
@@ -69,7 +71,7 @@ class ReduceTheFrequency
     // 500ms
     ros::Publisher trajectory_pub_,uav_mesh_pub_,ugv_path_pub_,ugv_odom_pub_;
     // 200ms
-    ros::Publisher odom_pub_,tf_pub_,optimal_list_pub_;
+    ros::Publisher odom_pub_,tf_pub_,optimal_list_pub_,ugv_optimal_list_pub_;
 
     double uav_position_x = 0,uav_position_y = 0;
 
@@ -84,6 +86,7 @@ class ReduceTheFrequency
     bool ugv_point_cloud_ready = false;
     bool ugv_odom_ready = false;
     bool ugv_path_ready = false;
+    bool ugv_optimal_list_ready = false;
 
     sensor_msgs::PointCloud2 octomap_point_cloud,octomap_compressed_point_cloud;
     sensor_msgs::PointCloud2 occupancy_inflate_point_cloud,occupancy_inflate_filtered_point_cloud,
@@ -94,7 +97,7 @@ class ReduceTheFrequency
     nav_msgs::Odometry odom,ugv_odom;
     tf2_msgs::TFMessage tf;
     visualization_msgs::Marker uav_mesh;
-    visualization_msgs::Marker optimal_list;
+    visualization_msgs::Marker optimal_list, ugv_optimal_list;
 
     ros::Timer send_timer_1000MS,send_timer_200MS,send_timer_500MS,send_timer_50MS;
 };

Modules/communication/launch/bridge.launch

@@ -83,7 +83,7 @@
         <!-- 单机配置文件加载 -->
         <rosparam command="load" file="$(arg prometheus_moudles_url)$(arg uav_control_yaml)"/>
         <!-- 集群配置文件加载 -->
-        <rosparam command="load" file="$(arg prometheus_moudles_url)$(arg swarm_control_yaml)"/>	
+        <!-- <rosparam command="load" file="$(arg prometheus_moudles_url)$(arg swarm_control_yaml)"/>	 -->
         <!-- 轨迹配置文件加载 -->
         <rosparam command="load" file="$(arg prometheus_moudles_url)$(arg uav_command_yaml)"/>
     </node>

Modules/communication/src/rviz_reduce_the_frequency.cpp

@@ -25,6 +25,7 @@ ReduceTheFrequency::ReduceTheFrequency(ros::NodeHandle &nh)
     ugv_point_cloud_sub_ = nh.subscribe("/ugv" + std::to_string(drone_id_) + "/cloud_registered",10,&ReduceTheFrequency::ugvPointCloudCb, this);
     ugv_odom_sub_ = nh.subscribe("/ugv" + std::to_string(drone_id_) + "/Odometry",10,&ReduceTheFrequency::ugvOdomCb, this);
     ugv_path_sub_ = nh.subscribe("/ugv" + std::to_string(drone_id_) + "/path",10,&ReduceTheFrequency::ugvPathCb, this);
+    ugv_optimal_list_sub_ = nh.subscribe("/ugv" + std::to_string(drone_id_) + "_ego_planner_node/optimal_list", 10 , &ReduceTheFrequency::ugvOptimalListCb, this);
 
     // 发布降低频率后的话题
     octomap_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/uav" + std::to_string(drone_id_) + "/octomap_point_cloud_centers/reduce_the_frequency", 100);
@@ -47,6 +48,7 @@ ReduceTheFrequency::ReduceTheFrequency(ros::NodeHandle &nh)
     ugv_point_cloud_pub_ = nh.advertise<sensor_msgs::PointCloud2>("/ugv" + std::to_string(drone_id_) + "/cloud_registered/reduce_the_frequency/compressed", 100);
     ugv_odom_pub_ = nh.advertise<nav_msgs::Odometry>("/ugv" + std::to_string(drone_id_) + "/Odometry/reduce_the_frequency", 100);
     ugv_path_pub_ = nh.advertise<nav_msgs::Path>("/ugv" + std::to_string(drone_id_) + "/path/reduce_the_frequency", 100);
+    ugv_optimal_list_pub_ = nh.advertise<visualization_msgs::Marker>("/ugv" + std::to_string(drone_id_) + "_ego_planner_node/optimal_list/reduce_the_frequency", 100);
 
     send_timer_1000MS = nh.createTimer(ros::Duration(1.0), &ReduceTheFrequency::send1000MS, this);
     usleep(10000);
@@ -146,6 +148,12 @@ void ReduceTheFrequency::send50MS(const ros::TimerEvent &time_event)
         optimal_list_pub_.publish(optimal_list);
         optimal_list_ready = false;
     }
+    usleep(100000);
+    if(ugv_optimal_list_ready)
+    {
+        ugv_optimal_list_pub_.publish(ugv_optimal_list);
+        ugv_optimal_list_ready = false;
+    }
 }
 
 void ReduceTheFrequency::octomapPointCloudCentersCb(const sensor_msgs::PointCloud2::ConstPtr &msg)
@@ -233,6 +241,12 @@ void ReduceTheFrequency::ugvPathCb(const nav_msgs::Path::ConstPtr &msg)
     ugv_path_ready = true;
 }
 
+void ReduceTheFrequency::ugvOptimalListCb(const visualization_msgs::Marker::ConstPtr &msg)
+{
+    ugv_optimal_list = *msg;
+    ugv_optimal_list_ready = true;
+}
+
 sensor_msgs::PointCloud2 ReduceTheFrequency::filtered(const sensor_msgs::PointCloud2 msg)
 {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);