# swarm_hebei

**Repository Path**: nics-robot/swarm_hebei

## Basic Information

- **Project Name**: swarm_hebei
- **Description**: swarm_hebei 
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2023-09-16
- **Last Updated**: 2023-09-17

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

下载源码
`git clone https://gitee.com/wwwpppccc/swarm_uwb.git`


配置环境

```
source /xxx/PX4-Autopilot/Tools/setup_gazebo.bash /xxx/PX4-Autopilot/ /xxx/PX4-Autopilot/build/px4_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/xxx/PX4-Autopilot
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:/xxx/PX4-Autopilot/Tools/sitl_gaze
```

安装ceres与glog与ddyanmic-reconfigure

```
cd 3rd_party/glog
./autogen.sh && ./configure && make && sudo make install
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3.1.2 libgflags-dev libgoogle-glog-dev libgtest-dev
cd ../ceres
mkdir build
cd build
cmake ..
sudo make install
sudo apt-get install ros-noetic-ddynamic-reconfigure
```

编译

```
cd ~/swarm_uwb
catkin_make
```

Quick Start​
`. Fast250/shfiles/autosim.sh`


启动多机实验

```
roslaunch px4 multi_mavros_dodging_ball.launch 【启动地图】
cd shfiles && ./swarm_dodging_sim.sh 【启动rviz】
roslaunch yolodetector sim_yolo_ros.launch 【启动球检测】
./drone0.sh 启动drone0
./drone1.sh 启动drone1
./drone2.sh 启动drone2
```


使用镜像的启动命令：

```
1. docker run -it -d -p 46789:46789 xxx(image的名字) 【创建container】
2. sudo service ssh start 【进入container后，打开ssh服务】
1. ssh nics@localhost -p 46789 -X  【密码是nics，这一步是把rviz和gazebo的图形界面转发过来】
2. roslaunch px4 multi_mavros_dodging_ball.launch 【启动地图】
3. cd shfiles && ./swarm_dodging_sim.sh 【启动rviz】
4. roslaunch yolodetector sim_yolo_ros.launch 【启动球检测】
5. ./drone0.sh 启动drone0
   ./drone1.sh 启动drone1
   ./drone2.sh 启动drone2
```


补充：docker常用命令如下


```
创建一个带名称的容器
    • docker create -it --name 容器名称 镜像

查看容器
    • docker ps – a

查看开启的容器
    • docker ps

启动容器
    • docker start 容器ID/NAME

进入容器
    • docker exec -it 容器ID/容器NAME /bin/bash

新建->启动->进入容器
    • docker run -it -p 7001:8001 centos:latest /bin/bash 

宿主机与容器之间复制文件
    • docker cp 宿主机绝对路径  容器id:路径
```


一些的代码说明：
- /home/nics/Drone/BambiplayGit-Fast-Drone-250-tuned_simulation/src/yolo_fdsst_piv/yolo_fastest_ros/ncnn/config/yolo_ros_sim.yaml


```
	配置了如下
	img_topic: /drone0/camera/rgb/image_raw（收到drone0的image）
	depth_topic: /drone0/camera/depth/image_raw （收到drone0的depth）
```


- /home/nics/Drone/BambiplayGit-Fast-Drone-250-tuned_simulation/src/yolo_fdsst_piv/yolo_fastest_ros/ncnn/src/rosnode_yolo.cpp


```
	void imageCb(const sensor_msgs::ImageConstPtr& msg)
		检测出球的中心位置
		void get_depth ()
			depth = reg_test.ptr<ushort>(roi.x+roi.width/2)[roi.y+roi.height/2];
			ball_pose_pub.publish(pose); //发布"ball/pose"
		image_pub_.publish(outputmsg); //发布"image_converter/output_video"

	void depthCb(const sensor_msgs::ImageConstPtr& msg)
```


- /home/nics/Drone/BambiplayGit-Fast-Drone-250-tuned_simulation/src/planner/plan_env/src/grid_map.cpp
```
    void GridMap::ballposeCb
	    接收“ball/pose” "/drone_0_ego_planner_node/ball/pose" , 10,&GridMap::ballposeCb, this);
	    位置存在pos_t里
    void GridMap::depthPoseCallback
	    md_.camera_pos_ 相机的位置
	    md_.camera_r_m_ 相机的姿态
    void GridMap::projectDepthImage()
	    proj_point(0) = (pos_t(0) - mp_.cx_) * depth_test / mp_.fx_;
    	    proj_point(1) = (pos_t(1) - mp_.cy_) * depth_test / mp_.fy_;
	    proj_point(2) = depth_test; //图像坐标系转到相机坐标系
	    proj_point = camera_r * proj_point + md_.camera_pos_; //相机坐标系转到世界坐标系
	    ball_pred_pub.publish(pose);// 发布话题"ball/pose_now"

	    pos_t 是像素坐标，proj_point是相机坐标系下的空间点，最后的proj_point 是世界坐标系下的	空间点，其中camera_r 是旋转矩阵，md_.camera_pos_是平移矩阵
	    drone_traj_sub_ = 	node_.subscribe<geometry_msgs::PoseStamped>("drone_traj_with_time" , 10, &GridMap::droneposeCb, this);
    void GridMap::droneposeCb(const geometry_msgs::PoseStampedConstPtr& msg) //接收无人机的轨迹
	    发布预测的球的轨迹"ball/prediction"
            md_.occupancy_buffer_inflate_设置为-1是还没被膨胀，然后拿-1去作膨胀，膨胀过后设为1，最后inflate地图会把非0的全部标上
    void GridMap::othermapCb(const sensor_msgs::PointCloud2ConstPtr &msg)
	    接收其他无人机的地图信息,并更新地图
```