From 7692abeb49247e3a3363e89d19d0ad69a50a26e1 Mon Sep 17 00:00:00 2001
From: "litian.zhuang" <litian.zhuang@nxrobo.com>
Date: Mon, 13 Dec 2021 15:59:33 +0800
Subject: [PATCH] point2laser is instead of depth2laser

---
 onekey.sh                                     | 22 +++++++-------
 .../launch/amcl_demo_rviz.launch              | 18 ++----------
 .../spark_rtabmap/launch/rtab_demo.launch     |  9 +++---
 .../launch/spark_mapping_astrapro.launch      |  4 +--
 .../launch/spark_rtabmap_nav.launch           |  7 +++--
 .../launch/spark_rtabmap_teleop.launch        |  4 +--
 .../spark_slam/launch/depth_slam.launch       | 23 +++------------
 .../launch/depth_slam_teleop.launch           |  2 --
 .../launch/astra_p2s.launch                   | 29 +++++++++++++++++++
 .../launch/astrapro_p2s.launch                | 29 +++++++++++++++++++
 .../launch/d435_p2s.launch                    | 29 +++++++++++++++++++
 11 files changed, 118 insertions(+), 58 deletions(-)
 create mode 100755 src/spark_driver/camera/camera_driver_transfer/launch/astra_p2s.launch
 create mode 100755 src/spark_driver/camera/camera_driver_transfer/launch/astrapro_p2s.launch
 create mode 100755 src/spark_driver/camera/camera_driver_transfer/launch/d435_p2s.launch

diff --git a/onekey.sh b/onekey.sh
index 58b7d05..0322d66 100755
--- a/onekey.sh
+++ b/onekey.sh
@@ -95,17 +95,17 @@ check_camera(){
 	#检查使用哪种设备
 	if [ -n "$(lsusb -d 2bc5:0403)" ]; then
 		CAMERATYPE="astrapro"
-		depthtolaser="/camera/depth/image_rect_raw"
+
 		camera_flag=$[$camera_flag + 1]
 	fi
 	if [ -n "$(lsusb -d 2bc5:0401)" ]; then
 		CAMERATYPE="astra"
-		depthtolaser="/camera/depth/image_raw"
+
 		camera_flag=$[$camera_flag + 1]
 	fi
 	if [ -n "$(lsusb -d 8086:0b07)" ]; then
 		CAMERATYPE="d435"
-		depthtolaser="/camera/depth/image_rect_raw"
+
 		camera_flag=$[$camera_flag + 1]
 	fi
 
@@ -507,11 +507,11 @@ spark_navigation_3d(){
 	echo -e "${Info}" 
 	echo && stty erase ^? && read -p "按回车键（Enter）开始：" 
 	if [[ "${SLAMTYPE}" == "2d" ]]; then
-		print_command "roslaunch spark_navigation amcl_demo_rviz.launch camera_type_tel:=${CAMERATYPE} depthtolaser:=${depthtolaser}"
-		roslaunch spark_navigation amcl_demo_rviz.launch camera_type_tel:=${CAMERATYPE} depthtolaser:=${depthtolaser}
+		print_command "roslaunch spark_navigation amcl_demo_rviz.launch camera_type_tel:=${CAMERATYPE}"
+		roslaunch spark_navigation amcl_demo_rviz.launch camera_type_tel:=${CAMERATYPE} 
 	else
-		print_command "roslaunch spark_rtabmap spark_rtabmap_nav.launch camera_type_tel:=${CAMERATYPE} depthtolaser:=${depthtolaser}"	
-		roslaunch spark_rtabmap spark_rtabmap_nav.launch camera_type_tel:=${CAMERATYPE} depthtolaser:=${depthtolaser}
+		print_command "roslaunch spark_rtabmap spark_rtabmap_nav.launch camera_type_tel:=${CAMERATYPE}"	
+		roslaunch spark_rtabmap spark_rtabmap_nav.launch camera_type_tel:=${CAMERATYPE} 
 	fi	
 }
 
@@ -833,14 +833,14 @@ spark_build_map_3d(){
 		echo -e "${Tip}" 
 		echo && stty erase ^? && read -p "请选择是否继续y/n：" choose
 		if [[ "${choose}" == "y" ]]; then
-			print_command "roslaunch spark_rtabmap spark_rtabmap_teleop.launch camera_type_tel:=${CAMERATYPE} depthtolaser:=${depthtolaser}"
-                	roslaunch spark_rtabmap spark_rtabmap_teleop.launch camera_type_tel:=${CAMERATYPE} depthtolaser:=${depthtolaser}
+			print_command "roslaunch spark_rtabmap spark_rtabmap_teleop.launch camera_type_tel:=${CAMERATYPE}"
+                	roslaunch spark_rtabmap spark_rtabmap_teleop.launch camera_type_tel:=${CAMERATYPE}
 		else
 			return
 		fi
         else
-        	print_command "roslaunch spark_slam depth_slam_teleop.launch slam_methods_tel:=${SLAMTYPE} camera_type_tel:=${CAMERATYPE} depthtolaser:=${depthtolaser}"
-		roslaunch spark_slam depth_slam_teleop.launch slam_methods_tel:=${SLAMTYPE} camera_type_tel:=${CAMERATYPE} depthtolaser:=${depthtolaser}
+        	print_command "roslaunch spark_slam depth_slam_teleop.launch slam_methods_tel:=${SLAMTYPE} camera_type_tel:=${CAMERATYPE}"
+		roslaunch spark_slam depth_slam_teleop.launch slam_methods_tel:=${SLAMTYPE} camera_type_tel:=${CAMERATYPE}
 	fi
 	
 }
diff --git a/src/spark_app/spark_navigation/launch/amcl_demo_rviz.launch b/src/spark_app/spark_navigation/launch/amcl_demo_rviz.launch
index be55d5d..6457657 100755
--- a/src/spark_app/spark_navigation/launch/amcl_demo_rviz.launch
+++ b/src/spark_app/spark_navigation/launch/amcl_demo_rviz.launch
@@ -3,7 +3,7 @@
 	<!-- 摄像机类型 -->
 	<arg name="camera_type_tel" default="astrapro" doc="camera type [astrapro, astra, d435...]"/>
 	<arg name="3d_sensor" default="astra"/>  <!-- r200, kinect, asus_xtion_pro -->
-	<arg name="depthtolaser"	default="/camera/depth/image_rect_raw"/>
+
 	
 	<!-- spark底盘驱动，机器人描述,底盘,相机 -->
 	<include file="$(find spark_bringup)/launch/driver_bringup.launch">
@@ -11,20 +11,8 @@
 	</include>
 	
 	<!-- Run the depthimage_to_laserscan node -->
-  	<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen">
-    
-    		<!--<remap from="image" to="/camera/depth/image_raw" />-->
-    		<remap from="image" to="$(arg depthtolaser)" />
-
-    		<!--<remap from="camera_info" to="/camera/depth/camera_info" />-->
-    		<remap from="camera_info" to="/camera/depth/camera_info" />
-
-    		<remap from="scan" to="/scan" />
-    		<param name="output_frame_id" value="camera_depth_frame" />
-
-    		<!--added by xilai 6/19/2017-->
-    		<param name="scan_height" value="100" />
-  	</node>
+	<include file="$(find camera_driver_transfer)/launch/$(arg camera_type_tel)_p2s.launch">
+	</include>
 
 
   	<!-- Map server -->
diff --git a/src/spark_app/spark_rtabmap/launch/rtab_demo.launch b/src/spark_app/spark_rtabmap/launch/rtab_demo.launch
index f0868a2..caabd84 100755
--- a/src/spark_app/spark_rtabmap/launch/rtab_demo.launch
+++ b/src/spark_app/spark_rtabmap/launch/rtab_demo.launch
@@ -2,16 +2,17 @@
 <launch>
   	<!-- camera -->
   	<arg name="camera_types" default="" doc="camera type [astrapro, astra]"/>
-	<arg name="point2laser"	default="/camera/depth/points"/>
+	<arg name="depthtolaser"	default="/camera/depth/points"/>
 
 	<!-- spark底盘驱动，机器人描述,底盘,相机 -->
 	<include file="$(find spark_bringup)/launch/driver_bringup.launch">
 		<arg name="camera_types"	value="$(arg camera_types)"/>
 		<arg name="dp_rgist"		value="true"/>
 	</include>
-	<include file="$(find spark_slam)/launch/pointcloud2scan.launch">
-		<arg name="point2laser"	value="$(arg point2laser)"/>
-	</include>	
+	<!-- Run the depthimage_to_laserscan node -->
+	<include file="$(find camera_driver_transfer)/launch/$(arg camera_types)_p2s.launch">
+	</include>
+
  
   	<!-- Move base -->
   	<include file="$(find spark_navigation)/launch/includes/move_base.launch.xml"/>
diff --git a/src/spark_app/spark_rtabmap/launch/spark_mapping_astrapro.launch b/src/spark_app/spark_rtabmap/launch/spark_mapping_astrapro.launch
index a2be635..8b07004 100755
--- a/src/spark_app/spark_rtabmap/launch/spark_mapping_astrapro.launch
+++ b/src/spark_app/spark_rtabmap/launch/spark_mapping_astrapro.launch
@@ -25,7 +25,7 @@
 	  <param name="wait_for_transform_duration"  type="double"   value="$(arg wait_for_transform)"/>
 	  <param name="subscribe_depth"     type="bool"   value="true"/>
 	  <param name="subscribe_scan"      type="bool"   value="true"/>
-	  <param name="map_negative_poses_ignored" type="bool" value="true"/>
+	  <param name="map_always_update" type="bool" value="true"/>
 
           <!-- When sending goals on /rtabmap/goal topic, use actionlib to communicate with move_base --> 
 	  <param name="use_action_for_goal" type="bool" value="true"/>
@@ -83,7 +83,7 @@
     <!-- visualization with rtabmapviz -->
     <node if="$(arg rtabmapviz)" pkg="rtabmap_ros" type="rtabmapviz" name="rtabmapviz" args="-d $(find rtabmap_ros)/launch/config/rgbd_gui.ini" output="screen">
   	  <param name="subscribe_depth"             type="bool" value="true"/>
-      <param name="subscribe_scan"              type="bool" value="true"/>
+      <param name="subscribe_scan"              type="bool" value="false"/>
       <param name="frame_id"                    type="string" value="base_footprint"/>
       <param name="wait_for_transform_duration" type="double" value="$(arg wait_for_transform)"/>
     
diff --git a/src/spark_app/spark_rtabmap/launch/spark_rtabmap_nav.launch b/src/spark_app/spark_rtabmap/launch/spark_rtabmap_nav.launch
index 061566c..46f0cda 100755
--- a/src/spark_app/spark_rtabmap/launch/spark_rtabmap_nav.launch
+++ b/src/spark_app/spark_rtabmap/launch/spark_rtabmap_nav.launch
@@ -3,7 +3,7 @@
 <launch>
 	<!-- camera -->
 	<arg name="camera_type_tel" default="" doc="camera type [astrapro, astra]"/>
-	<arg name="point2laser" default="/camera/depth_registered/points"/>
+
 	
 	<!-- spark底盘驱动，机器人描述,底盘,相机 -->
 	<include file="$(find spark_bringup)/launch/driver_bringup.launch">
@@ -12,11 +12,12 @@
 	</include>
 
 
-  	<include file="$(find spark_slam)/launch/pointcloud2scan.launch">
-		<arg name="point2laser"	value="$(arg point2laser)"/>
+	<!-- Run the depthimage_to_laserscan node -->
+	<include file="$(find camera_driver_transfer)/launch/$(arg camera_type_tel)_p2s.launch">
 	</include>
 
 
+
   	<!-- Move base -->
   	<include file="$(find spark_navigation)/launch/includes/move_base.launch.xml"/>
 
diff --git a/src/spark_app/spark_rtabmap/launch/spark_rtabmap_teleop.launch b/src/spark_app/spark_rtabmap/launch/spark_rtabmap_teleop.launch
index 3a70030..285ce51 100755
--- a/src/spark_app/spark_rtabmap/launch/spark_rtabmap_teleop.launch
+++ b/src/spark_app/spark_rtabmap/launch/spark_rtabmap_teleop.launch
@@ -1,13 +1,13 @@
 <!--spark rtabmap-->
 <!--SPARK技术讨论与反馈群：8346256-->
 <launch>
-	<arg name="point2laser" default="/camera/depth_registered/points"/>
+
 	<!-- camera -->
 	<arg name="camera_type_tel" default="" doc="camera type [astrapro, astra]" />
 	<!-- spark base driver -->
 	<include file="$(find spark_rtabmap)/launch/rtab_demo.launch">
 		<arg name="camera_types"	value="$(arg camera_type_tel)" />
-		<arg name="point2laser"	value="$(arg point2laser)"/>	
+
 	</include>
 
 	<!-- rtabmap -->
diff --git a/src/spark_app/spark_slam/launch/depth_slam.launch b/src/spark_app/spark_slam/launch/depth_slam.launch
index 1efa982..77ec715 100755
--- a/src/spark_app/spark_slam/launch/depth_slam.launch
+++ b/src/spark_app/spark_slam/launch/depth_slam.launch
@@ -2,7 +2,6 @@
 <!--SPARK技术讨论与反馈群：8346256-->
 <launch>
   	<!-- Arguments -->
-	<arg name="depthtolaser"	default="/camera/depth/image_rect_raw"/>
   	<arg name="slam_methods" default="gmapping" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/>
   	<arg name="camera_types" default="astrapro" doc="camera type [astrapro, astra, d435]"/>
   	<arg name="configuration_basename" default="spark_lds_2d.lua"/>
@@ -12,25 +11,11 @@
 	<include file="$(find spark_bringup)/launch/driver_bringup.launch">
 		<arg name="camera_types" 	value="$(arg camera_types)"/>
 	</include>
+	
+	<!-- Run the depthimage_to_laserscan node -->
+	<include file="$(find camera_driver_transfer)/launch/$(arg camera_types)_p2s.launch">
+	</include>
 
-  	<!-- Run the depthimage_to_laserscan node -->
-	<include file="$(find spark_slam)/launch/pointcloud2scan.launch"/>
-  	<!--
-  	<node pkg="depthimage_to_laserscan" type="depthimage_to_laserscan" name="depthimage_to_laserscan" output="screen">
-    
-
-    		<remap from="image" to="$(arg depthtolaser)" />
-
-    		
-    		<remap from="camera_info" to="/camera/depth/camera_info" />
-
-    		<remap from="scan" to="/scan" />
-    		<param name="output_frame_id" value="camera_depth_frame" />
-
-    		
-    		<param name="scan_height" value="50" />
-  	</node>
-	-->
   	<!-- SLAM: Gmapping, Cartographer, Hector, Karto, Frontier_exploration, RTAB-Map -->
   	<include file="$(find spark_slam)/launch/spark_$(arg slam_methods).launch">
     	<!--arg name="model" value="$(arg model)"/-->
diff --git a/src/spark_app/spark_slam/launch/depth_slam_teleop.launch b/src/spark_app/spark_slam/launch/depth_slam_teleop.launch
index 2671509..6b8338c 100755
--- a/src/spark_app/spark_slam/launch/depth_slam_teleop.launch
+++ b/src/spark_app/spark_slam/launch/depth_slam_teleop.launch
@@ -2,12 +2,10 @@
 <launch>
 	<!--spark slam-->
   	<arg name="slam_methods_tel" default="gmapping" doc="slam type [gmapping, cartographer, hector, karto, frontier_exploration]"/>
-	<arg name="depthtolaser" default="/camera/depth/image_rect_raw"/>
 	<arg name="camera_type_tel" default="" doc="camera type [astrapro, astra]"/>
 	<include file="$(find spark_slam)/launch/depth_slam.launch">
 		<arg name="slam_methods"	value="$(arg slam_methods_tel)"/>
 		<arg name="camera_types"	value="$(arg camera_type_tel)" />
-		<arg name="depthtolaser"	value="$(arg depthtolaser)"/>
  	</include> 
   	<!--创建新的终端，spark键盘控制 “wsad”分别代表“前后左右”-->
   	<node pkg="spark_teleop" type="keyboard_control.sh" name="kc_depth" />
diff --git a/src/spark_driver/camera/camera_driver_transfer/launch/astra_p2s.launch b/src/spark_driver/camera/camera_driver_transfer/launch/astra_p2s.launch
new file mode 100755
index 0000000..cde53d2
--- /dev/null
+++ b/src/spark_driver/camera/camera_driver_transfer/launch/astra_p2s.launch
@@ -0,0 +1,29 @@
+<!--astra -->
+<!--spark camera pointcloud to laserscan-->
+<launch>
+    <arg name="point2laser"	default="/camera/depth/points"/>
+    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
+      <remap from="cloud_in" to="$(arg point2laser)"/>
+      <remap from="scan" to="/scan" />
+      <rosparam>
+        target_frame: camera_link
+        transform_tolerance: 0.01
+        min_height: 0.0
+        max_height: 1.0
+        
+        angle_min: -1.5708
+        angle_max: 1.5708
+        angle_increment: 0.0087
+        scan_time: 0.3333
+        range_min: 0.45
+        range_max: 4.0
+        use_inf: true
+        
+        #concurrency_level affects number of pc queued for processing and the number of threadsused
+        # 0: Detect number of cores
+        # 1: Single threaded
+        # 2: inf : Parallelism level
+        concurrency_level: 1
+      </rosparam>
+    </node>
+</launch>
diff --git a/src/spark_driver/camera/camera_driver_transfer/launch/astrapro_p2s.launch b/src/spark_driver/camera/camera_driver_transfer/launch/astrapro_p2s.launch
new file mode 100755
index 0000000..cde53d2
--- /dev/null
+++ b/src/spark_driver/camera/camera_driver_transfer/launch/astrapro_p2s.launch
@@ -0,0 +1,29 @@
+<!--astra -->
+<!--spark camera pointcloud to laserscan-->
+<launch>
+    <arg name="point2laser"	default="/camera/depth/points"/>
+    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
+      <remap from="cloud_in" to="$(arg point2laser)"/>
+      <remap from="scan" to="/scan" />
+      <rosparam>
+        target_frame: camera_link
+        transform_tolerance: 0.01
+        min_height: 0.0
+        max_height: 1.0
+        
+        angle_min: -1.5708
+        angle_max: 1.5708
+        angle_increment: 0.0087
+        scan_time: 0.3333
+        range_min: 0.45
+        range_max: 4.0
+        use_inf: true
+        
+        #concurrency_level affects number of pc queued for processing and the number of threadsused
+        # 0: Detect number of cores
+        # 1: Single threaded
+        # 2: inf : Parallelism level
+        concurrency_level: 1
+      </rosparam>
+    </node>
+</launch>
diff --git a/src/spark_driver/camera/camera_driver_transfer/launch/d435_p2s.launch b/src/spark_driver/camera/camera_driver_transfer/launch/d435_p2s.launch
new file mode 100755
index 0000000..800270b
--- /dev/null
+++ b/src/spark_driver/camera/camera_driver_transfer/launch/d435_p2s.launch
@@ -0,0 +1,29 @@
+<!--astra -->
+<!--spark camera pointcloud to laserscan-->
+<launch>
+    <arg name="point2laser"	default="/camera/depth_registered/points"/>
+    <node pkg="pointcloud_to_laserscan" type="pointcloud_to_laserscan_node" name="pointcloud_to_laserscan">
+      <remap from="cloud_in" to="$(arg point2laser)"/>
+      <remap from="scan" to="/scan" />
+      <rosparam>
+        target_frame: camera_link
+        transform_tolerance: 0.01
+        min_height: 0.0
+        max_height: 1.0
+        
+        angle_min: -1.5708
+        angle_max: 1.5708
+        angle_increment: 0.0087
+        scan_time: 0.3333
+        range_min: 0.45
+        range_max: 4.0
+        use_inf: true
+        
+        #concurrency_level affects number of pc queued for processing and the number of threadsused
+        # 0: Detect number of cores
+        # 1: Single threaded
+        # 2: inf : Parallelism level
+        concurrency_level: 1
+      </rosparam>
+    </node>
+</launch>