map_scan_match package publish tf, ceres match with new map from gedian.

file_list.txt

@@ -0,0 +1,49 @@
+display_lidar_points
+livox雷达sdk封装，连接后自动发布点云
+
+gazebo_robot
+gazebo仿真工具使用demo，功能未完成
+
+grapher
+使用icp与ndt通过steger算法与地图匹配定位与制图的slam基本模组
+
+map_scan_match
+仿造cartographer前端的点云与地图匹配模组，读入yaml类型map，订阅scan，发布匹配后点云与tf
+
+move_base
+ros平台提供的全局/局部路径规划与运动控制算法
+
+MPC
+使用cppAD进行非线性优化，实现单/双AGV模型预测控制
+
+mpc_control
+自行实现的同样使用cppAD非线性优化进行运动控制，并使用Eigen库三阶线性插值生成全局路径的模组。此模组中双AGV控制未完成，在MPC模组中已实现。
+
+pepperl_fuchs_r2000
+倍加福r2000雷达ros包
+
+rbx1
+ros平台提供的完善的路径规划与导航算法栈，该模组只包含配置与launch文件，不含源码
+
+robot_control
+读取move_base生成的线速度与角速度值，通过modbus下发到PLC控制电机运动
+
+robot_sim
+用于测试urdf机器人模型描述文件的使用，未完成
+
+scan_converter
+读取tf与scan，转换成点云发布
+
+scan_matcher_ICP
+ros提供的帧间匹配icp算法实现源码
+
+send_goal
+根据输入固定发布两个目标点坐标，供move_base导航到特定位置
+
+sick_tim
+sick雷达ros包
+
+urg_node
+北洋雷达(hokuyo)ros包
+
+

map_scan_match/200511map_origin.yaml

@@ -1,6 +0,0 @@
-image: /home/youchen/Documents/git_ws/src/map_scan_match/200511map_origin.pgm
-resolution: 0.050000
-origin: [-11.395653, -10.088187, 0.0]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196

map_scan_match/200521map_origin.yaml

@@ -0,0 +1,6 @@
+image: /home/youchen/Documents/git_ws/src/map_scan_match/200521map_origin.pgm
+resolution: 0.010000
+origin: [-19.701677, -9.709916, 0.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196

map_scan_match/configuration_files/demo.rviz

@@ -7,6 +7,10 @@ Panels:
         - /Global Options1
         - /Grid1
         - /PointCloud21
+        - /TF1
+        - /TF1/Frames1
+        - /TF1/Frames1/base_link1
+        - /TF1/Frames1/base_link1/Position1
       Splitter Ratio: 0.5
     Tree Height: 446
   - Class: rviz/Selection
@@ -91,6 +95,26 @@ Visualization Manager:
       Use Fixed Frame: true
       Use rainbow: true
       Value: true
+    - Class: rviz/TF
+      Enabled: true
+      Frame Timeout: 15
+      Frames:
+        All Enabled: true
+        base_link:
+          Value: true
+        map:
+          Value: true
+      Marker Scale: 1
+      Name: TF
+      Show Arrows: true
+      Show Axes: true
+      Show Names: true
+      Tree:
+        map:
+          base_link:
+            {}
+      Update Interval: 0
+      Value: true
   Enabled: true
   Global Options:
     Background Color: 48; 48; 48
@@ -116,7 +140,7 @@ Visualization Manager:
   Views:
     Current:
       Class: rviz/Orbit
-      Distance: 6.67555141
+      Distance: 8.12017441
       Enable Stereo Rendering:
         Stereo Eye Separation: 0.0599999987
         Stereo Focal Distance: 1

map_scan_match/map_0.05.yaml

@@ -1,6 +0,0 @@
-image: /home/youchen/Documents/git_ws/src/map_scan_match/map_0.05.pgm
-resolution: 0.050000
-origin: [-8.141946, -14.800098, 0.0]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196

map_scan_match/map_origin.yaml

@@ -1,6 +0,0 @@
-image: /home/youchen/Documents/catkin_ws/map_origin.pgm
-resolution: 0.050000
-origin: [-6.119765, -14.900000, 0.0]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196

map_scan_match/map_scan.launch

@@ -1,5 +1,5 @@
 <launch>
-  <node name="map_server" pkg="map_server" type="map_server" args="$(find map_scan_match)/200511map_origin.yaml"/>
+  <node name="map_server" pkg="map_server" type="map_server" args="$(find map_scan_match)/200521map_origin.yaml"/>
   <node name="rviz" pkg="rviz" type="rviz" required="true"
       args="-d $(find map_scan_match)/configuration_files/demo.rviz" />
   <!--node name="map_scan_matcher" pkg="map_scan_match" type="map_scan_match_node"/-->

map_scan_match/src/map_scan_match_node.cpp

@@ -64,10 +64,10 @@ int main(int argc, char** argv)
 //   cout<<"c= "<<abc[2]<<endl;
 
 ros::init(argc, argv, "map_scan_matcher");
-Scan_match sm("/home/youchen/Documents/git_ws/src/map_scan_match/200511map_origin.yaml");
+Scan_match sm("/home/youchen/Documents/git_ws/src/map_scan_match/200521map_origin.yaml");
 // sm.Solve();
 
-ros::Rate loop_rate(40);
+ros::Rate loop_rate(60);
 while(ros::ok()){
 
   // Eigen::Vector3d trans;

map_scan_match/src/scan_match.cpp

@@ -2,7 +2,7 @@
  * @Description: match scan with grid map
  * @Author: yct
  * @Date: 2020-04-03 13:10:37
- * @LastEditTime: 2020-05-17 23:11:23
+ * @LastEditTime: 2020-05-21 14:52:11
  * @LastEditors: yct
  */
 
@@ -53,7 +53,7 @@ Scan_match::Scan_match(std::string filename)
     m_initial_pose_subscriber = m_nh.subscribe("/initialpose", 1, &Scan_match::initial_pose_callback, this);
     m_cloud_publisher = m_nh.advertise<sensor_msgs::PointCloud2>("match_cloud", 10);
     m_cloud = pcl::PointCloud<pcl::PointXYZ>::Ptr(new pcl::PointCloud<pcl::PointXYZ>);
-    read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
+    //read_pointcloud("/home/youchen/Documents/git_ws/src/map_scan_match/my1.txt", m_cloud);
     m_trans << 0.0, 0.0, 0.0;
     if (read_map(filename))
     {
@@ -97,7 +97,7 @@ bool Scan_match::Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::P
     ceres::Problem problem;
     problem.AddResidualBlock(
         new ceres::AutoDiffCostFunction<BicubicInterpolationCost, ceres::DYNAMIC, 3>(
-            new BicubicInterpolationCost(m_probability_array, cloud, m_resolution, m_map_origin, 10.0 / sqrt(static_cast<double>(cloud->size()))), cloud->size()),
+            new BicubicInterpolationCost(m_probability_array, cloud, m_resolution, m_map_origin, 50.0 / sqrt(static_cast<double>(cloud->size()))), cloud->size()),
         nullptr,
         trans.data());
 
@@ -109,7 +109,7 @@ bool Scan_match::Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::P
 
     problem.AddResidualBlock(
         new ceres::AutoDiffCostFunction<RotationCost, 1, 3>(
-            new RotationCost(trans[2], 40.0)),
+            new RotationCost(trans[2], 10.0)),
         nullptr,
         trans.data());
 
@@ -141,6 +141,13 @@ bool Scan_match::Solve(Eigen::Vector3d &trans, pcl::PointCloud<pcl::PointXYZ>::P
     pcl::toROSMsg(*temp_cloud, output_origin);
     output_origin.header.frame_id = "map";
     m_cloud_publisher.publish(output_origin);
+
+    tf::TransformBroadcaster tf_broadcaster_;
+    tf::Transform transform;
+    transform.setOrigin(tf::Vector3(trans[0], trans[1], 0.0));
+    transform.setRotation(tf::Quaternion(0, 0, sin(trans[2]/2.0), cos(trans[2]/2.0)));
+    tf::StampedTransform transform_msg(transform, ros::Time::now(), "map", "base_link");
+    tf_broadcaster_.sendTransform(transform_msg);
 }
 
 bool Scan_match::Solve()

map_scan_match/src/scan_match.h

@@ -2,7 +2,7 @@
  * @Description: match scan with grid map
  * @Author: yct
  * @Date: 2020-04-03 11:50:01
- * @LastEditTime: 2020-05-17 23:05:08
+ * @LastEditTime: 2020-05-18 11:56:49
  * @LastEditors: yct
  */
 #ifndef SCAN_MATCH_HH