From 62c7618ac55f1d1214f5fd65609f502ccf0e4602 Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Sat, 27 Oct 2018 11:17:58 +0200
Subject: [PATCH] switch from xtion 3d camera to rplidar

---
 launch/wild_thumper.launch   | 13 +++++++++
 urdf/wild_thumper.urdf.xacro | 51 ++++++++++--------------------------
 2 files changed, 27 insertions(+), 37 deletions(-)

diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index a1abfaf..834a588 100644
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -57,4 +57,17 @@
 
 	<node pkg="wild_thumper" type="sensor_board.py" name="sensor_board" output="screen"/>
 	<node pkg="wild_thumper" type="ledstripe.rb" name="led_stripe" output="screen"/>
+
+	<node name="rplidarNode" pkg="rplidar_ros" type="rplidarNode" output="screen">
+		<param name="serial_port"         type="string" value="/dev/rplidar"/>
+		<param name="serial_baudrate"     type="int"    value="115200"/><!--A1/A2 -->
+		<param name="frame_id"            type="string" value="lidar"/>
+		<param name="angle_compensate"    type="bool"   value="true"/>
+		<param name="scan_mode"		  type="string"	value="Standard"/>
+		<!--
+		A2 scan_modes:
+		Standard: max_distance: 16.0 m, Point number: 2.0K
+		Express: max_distance: 16.0 m, Point number: 4.0K
+		 -->
+	</node>
 </launch>
diff --git a/urdf/wild_thumper.urdf.xacro b/urdf/wild_thumper.urdf.xacro
index 8be843c..5d5f9b7 100644
--- a/urdf/wild_thumper.urdf.xacro
+++ b/urdf/wild_thumper.urdf.xacro
@@ -1,7 +1,6 @@
 <?xml version="1.0"?>
 <robot name="wild_thumper" xmlns:xacro="http://ros.org/wiki/xacro">
 	<xacro:property name="PI" value="3.1415926535897931" />
-	<xacro:include filename="$(find hector_sensors_description)/urdf/asus_camera.urdf.xacro" />
 
 	<link name="base_footprint">
 		<visual>
@@ -52,9 +51,17 @@
 		</visual>
 	</link>
 
-	<xacro:asus_camera name="camera" parent="mounting_plate">
-		<origin xyz="0.107 0.0 0.04" rpy="0 0 0"/>
-	</xacro:asus_camera>
+	<link name="lidar">
+		<visual>
+			<geometry>
+				<mesh filename="package://wild_thumper/meshes/rplidar_a2.stl" scale="0.001 0.001 0.001"/>
+			</geometry>
+			<origin xyz="0.0 0.0 ${0.0408/2}" rpy="0 0 0"/>
+			<material name="black">
+				<color rgba="0 0 0 1"/>
+			</material>
+		</visual>
+	</link>
 
 	<link name="sonar_forward_left">
 		<visual>
@@ -92,30 +99,6 @@
 		</visual>
 	</link>
 
-	<link name="ir_left">
-		<visual>
-			<geometry>
-				<box size="0.015 0.015 0.046"/>
-			</geometry>
-			<origin xyz="${-0.015/2} 0 0" rpy="0 0 0"/>
-			<material name="black">
-				<color rgba="0 0 0 1"/>
-			</material>
-		</visual>
-	</link>
-
-	<link name="ir_right">
-		<visual>
-			<geometry>
-				<box size="0.015 0.015 0.046"/>
-			</geometry>
-			<origin xyz="${-0.015/2} 0 0" rpy="0 0 0"/>
-			<material name="black">
-				<color rgba="0 0 0 1"/>
-			</material>
-		</visual>
-	</link>
-
 	<joint name="base_link_joint" type="fixed">
 		<parent link="base_footprint"/>
 		<child link="base_link"/>
@@ -152,16 +135,10 @@
 		<origin xyz="-0.115 0.0 -0.012" rpy="0 ${-175*PI/180} 0"/>
 	</joint>
 
-	<joint name="ir_left_joint" type="fixed">
-		<parent link="mounting_plate"/>
-		<child link="ir_left"/>
-		<origin xyz="0.0 ${0.072+0.015} -0.045" rpy="0 0 ${PI/2}"/>
-	</joint>
-
-	<joint name="ir_right_joint" type="fixed">
+	<joint name="lidar_joint" type="fixed">
 		<parent link="mounting_plate"/>
-		<child link="ir_right"/>
-		<origin xyz="0.0 ${-0.072-0.015} -0.045" rpy="0 0 ${-PI/2}"/>
+		<child link="lidar"/>
+		<origin xyz="0.075 0.01 0.0" rpy="0 0 ${-110*PI/180}"/>
 	</joint>
 
 	<xacro:macro name="wheel" params="pos side xyz rpy">   
-- 
2.39.2