| 1872 | } |
| 1873 | |
| 1874 | int main(int argc, char **argv) |
| 1875 | { |
| 1876 | ros::init(argc, argv, "pcl_render"); |
| 1877 | ros::NodeHandle nh("~"); |
| 1878 | |
| 1879 | nh.param("quadrotor_name", quad_name, std::string("quadrotor")); |
| 1880 | nh.getParam("is_360lidar", is_360lidar); |
| 1881 | nh.getParam("sensing_horizon", sensing_horizon); |
| 1882 | nh.getParam("sensing_rate", sensing_rate); |
| 1883 | nh.getParam("estimation_rate", estimation_rate); |
| 1884 | nh.getParam("polar_resolution", polar_resolution); |
| 1885 | nh.getParam("yaw_fov", yaw_fov); |
| 1886 | nh.getParam("vertical_fov", vertical_fov); |
| 1887 | nh.getParam("min_raylength", min_raylength); |
| 1888 | nh.getParam("downsample_res", downsample_res); |
| 1889 | nh.getParam("livox_linestep", livox_linestep); |
| 1890 | nh.getParam("use_avia_pattern", use_avia_pattern); |
| 1891 | nh.getParam("curvature_limit", curvature_limit); |
| 1892 | nh.getParam("hash_cubesize", hash_cubesize); |
| 1893 | nh.getParam("use_vlp32_pattern", use_vlp32_pattern); |
| 1894 | nh.getParam("use_minicf_pattern", use_minicf_pattern); |
| 1895 | |
| 1896 | // dyn parameters |
| 1897 | nh.getParam("dynobj_enable", dynobj_enable); |
| 1898 | nh.getParam("dynobject_size", dynobject_size); |
| 1899 | nh.getParam("dynobject_num", dynobject_num); |
| 1900 | nh.getParam("dyn_mode", dyn_mode); |
| 1901 | nh.getParam("dyn_velocity", dyn_velocity); |
| 1902 | |
| 1903 | nh.getParam("use_uav_extra_model", use_uav_extra_model); |
| 1904 | |
| 1905 | nh.getParam("collisioncheck_enable", collisioncheck_enable); |
| 1906 | nh.getParam("collision_range", collision_range); |
| 1907 | |
| 1908 | nh.getParam("output_pcd", output_pcd); |
| 1909 | |
| 1910 | // subscribe other uav pos |
| 1911 | nh.param("uav_num", drone_num, 1); |
| 1912 | nh.param("drone_id", drone_id, 0); |
| 1913 | nh.param("lidar_pitch", lidar_pitch, 0.0); |
| 1914 | other_uav_pos.resize(drone_num); |
| 1915 | other_uav_rcv_time.resize(drone_num); |
| 1916 | otheruav_points.resize(drone_num); |
| 1917 | otheruav_pointsindex.resize(drone_num); |
| 1918 | ros::Subscriber *subs = new ros::Subscriber[drone_num]; |
| 1919 | for (int i = 0; i < drone_num; i++) |
| 1920 | { |
| 1921 | if (i == drone_id) |
| 1922 | { |
| 1923 | continue; |
| 1924 | } |
| 1925 | string topic = "/quad_"; |
| 1926 | topic += to_string(i); |
| 1927 | topic += "/lidar_slam/odom"; |
| 1928 | cout << topic << endl; |
| 1929 | subs[i] = nh.subscribe<nav_msgs::Odometry>(topic, 1000, boost::bind(&multiOdometryCallbck, _1, i)); |
| 1930 | } |
| 1931 | |