From b301eb19e6487497a8bb946c538a25aa4ee28c1d Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Mon, 19 Jun 2017 19:30:37 +0200
Subject: [PATCH] =?utf8?q?Set=20sonar=20angle=20to=205=C2=B0=20degree?=
MIME-Version: 1.0
Content-Type: text/plain; charset=utf8
Content-Transfer-Encoding: 8bit

---
 avr/motor_ctrl/main.c            | 13 +++++++------
 avr/motor_ctrl/main.hex          | 16 ++++++++--------
 config/wt_node.cfg               |  2 +-
 launch/robot_localization.launch | 13 +++++++++----
 launch/wild_thumper.launch       |  6 +++---
 scripts/sensor_board.py          |  4 ++--
 scripts/wt_node.py               |  6 +++---
 urdf/wild_thumper.urdf.xacro     |  6 +++---
 8 files changed, 36 insertions(+), 30 deletions(-)

diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c
index 16c8b0c..5d65529 100644
--- a/avr/motor_ctrl/main.c
+++ b/avr/motor_ctrl/main.c
@@ -97,8 +97,8 @@
  */
 
 
-#define KP 0.062
-#define KI 0.12
+#define KP 0.09
+#define KI 0.07
 #define KD 0.0
 #define PID_T 0.01
 // wheel diameter=12cm, encoder=48cpr, gear ratio=1:47
@@ -109,6 +109,7 @@
 #define STEP_PER_M_RIGHT (STEP_PER_M)
 #define WHEEL_DIST 0.39912 // Measured: 0.252
 #define PWM_BREAK INT16_MIN
+#define STALL_LIMIT 140000
 
 #define TWI_ACK   TWCR = (1<<TWEA) | (1<<TWINT) | (1<<TWEN) | (1<<TWIE)
 #define TWI_RESET TWCR &= ~((1 << TWSTO) | (1 << TWEN)); TWI_ACK
@@ -899,25 +900,25 @@ static void update_pid(void) {
 	static int32_t esum4=0;
 
 	// protect motors from damage if stalling
-	if (labs(esum1) > 140000 && speed1 == 0) {
+	if (labs(esum1) > STALL_LIMIT && speed1 == 0) {
 		motor1 = 0;
 		motor1_mode = MOTOR_MANUAL;
 		error_state |= (1<<4);
 		esum1 = 0;
 	}	
-	if (labs(esum2) > 140000 && speed2 == 0) {
+	if (labs(esum2) > STALL_LIMIT && speed2 == 0) {
 		motor2 = 0;
 		motor2_mode = MOTOR_MANUAL;
 		error_state |= (1<<5);
 		esum2 = 0;
 	}	
-	if (labs(esum3) > 140000 && speed3 == 0) {
+	if (labs(esum3) > STALL_LIMIT && speed3 == 0) {
 		motor3 = 0;
 		motor3_mode = MOTOR_MANUAL;
 		error_state |= (1<<6);
 		esum3 = 0;
 	}	
-	if (labs(esum4) > 140000 && speed4 == 0) {
+	if (labs(esum4) > STALL_LIMIT && speed4 == 0) {
 		motor4 = 0;
 		motor4_mode = MOTOR_MANUAL;
 		error_state |= (1<<7);
diff --git a/avr/motor_ctrl/main.hex b/avr/motor_ctrl/main.hex
index ec2e8cc..aef12fe 100644
--- a/avr/motor_ctrl/main.hex
+++ b/avr/motor_ctrl/main.hex
@@ -321,9 +321,9 @@
 :10140000B90AB501882777FD8095982FC0908E0086
 :10141000D0908F00E0909000F0909100C60ED71E03
 :10142000E81EF91EC0928E00D0928F00E0929000CC
-:10143000F09291000E94FC1026EB33EF4DE75DE344
+:10143000F09291000E94FC102CEE31E548EB5DE348
 :101440000E94E9112B013C01C701B6010E94FC106A
-:1014500021E539E44DE95AE30E94E9119B01AC0111
+:1014500024E330E847E35AE30E94E9119B01AC0121
 :10146000C301B2010E94F60F6B017C0180918000E4
 :1014700090918100B501681B790B882777FD8095D5
 :10148000982F0E94FC1020E030E0A9010E94E91191
@@ -349,9 +349,9 @@
 :1015C000B90AB501882777FD8095982FC0908A00C9
 :1015D000D0908B00E0908C00F0908D00C60ED71E4E
 :1015E000E81EF91EC0928A00D0928B00E0928C0017
-:1015F000F0928D000E94FC1026EB33EF4DE75DE387
+:1015F000F0928D000E94FC102CEE31E548EB5DE38B
 :101600000E94E9112B013C01C701B6010E94FC10A8
-:1016100021E539E44DE95AE30E94E9119B01AC014F
+:1016100024E330E847E35AE30E94E9119B01AC015F
 :10162000C301B2010E94F60F6B017C0180917E0024
 :1016300090917F00B501681B790B882777FD809515
 :10164000982F0E94FC1020E030E0A9010E94E911CF
@@ -377,9 +377,9 @@
 :10178000B90AB501882777FD8095982FC09086000B
 :10179000D0908700E0908800F0908900C60ED71E98
 :1017A000E81EF91EC0928600D0928700E092880061
-:1017B000F09289000E94FC1026EB33EF4DE75DE3C9
+:1017B000F09289000E94FC102CEE31E548EB5DE3CD
 :1017C0000E94E9112B013C01C701B6010E94FC10E7
-:1017D00021E539E44DE95AE30E94E9119B01AC018E
+:1017D00024E330E847E35AE30E94E9119B01AC019E
 :1017E000C301B2010E94F60F6B017C0180917C0065
 :1017F00090917D00B501681B790B882777FD809556
 :10180000982F0E94FC1020E030E0A9010E94E9110D
@@ -405,9 +405,9 @@
 :10194000B90AB501882777FD8095982FC09082004D
 :10195000D0908300E0908400F0908500C60ED71EE2
 :10196000E81EF91EC0928200D0928300E0928400AB
-:10197000F09285000E94FC1026EB33EF4DE75DE30B
+:10197000F09285000E94FC102CEE31E548EB5DE30F
 :101980000E94E9112B013C01C701B6010E94FC1025
-:1019900021E539E44DE95AE30E94E9119B01AC01CC
+:1019900024E330E847E35AE30E94E9119B01AC01DC
 :1019A000C301B2010E94F60F6B017C0180917A00A5
 :1019B00090917B00B501681B790B882777FD809596
 :1019C000982F0E94FC1020E030E0A9010E94E9114C
diff --git a/config/wt_node.cfg b/config/wt_node.cfg
index 725c565..aad0d54 100755
--- a/config/wt_node.cfg
+++ b/config/wt_node.cfg
@@ -6,7 +6,7 @@ gen = ParameterGenerator()
 
 gen.add("range_sensor_clip",	bool_t,		0, "Clip range sensor values to max range", True)
 gen.add("range_sensor_max",	double_t,	0, "Range sensor max range", 0.5, 0.04, 4)
-gen.add("odom_covar_xy",	double_t,	0, "Odometry covariance: translation", 1e-2, 1e-6, 1)
+gen.add("odom_covar_xy",	double_t,	0, "Odometry covariance: translation", 1e-3, 1e-6, 1)
 gen.add("odom_covar_angle",	double_t,	0, "Odometry covariance: rotation", 1.00, 1e-6, 6.2832)
 gen.add("rollover_protect",	bool_t,		0, "Enable motor rollover protection on pitch", True)
 gen.add("rollover_protect_limit",double_t,	0, "Pitch rollover protection limit (degree)", 45, 0, 90)
diff --git a/launch/robot_localization.launch b/launch/robot_localization.launch
index d3da317..0a7aa6d 100644
--- a/launch/robot_localization.launch
+++ b/launch/robot_localization.launch
@@ -21,7 +21,7 @@
 		   carry out a predict cycle on the EKF without correcting it. This parameter can be thought of
 		   as the minimum frequency with which the filter will generate new output. Defaults to 1 / frequency
 		   if not specified. -->
-		<param name="sensor_timeout" value="2.0"/>
+		<!-- <param name="sensor_timeout" value="2.0"/> -->
 
 		<!-- If this is set to true, no 3D information will be used in your state estimate. Use this if you
 		   are operating in a planar environment and want to ignore the effect of small variations in the
@@ -59,7 +59,7 @@
 		<!-- Use this parameter to provide an offset to the transform generated by ekf_localization_node. This
 		   can be used for future dating the transform, which is required for interaction with some other
 		   packages. Defaults to 0.0 if unspecified. -->
-		<param name="transform_time_offset" value="0.0"/>
+		<!-- <param name="transform_time_offset" value="0.0"/> -->
 
 		<!-- The filter accepts an arbitrary number of inputs from each input message type (Odometry, PoseStamped,
 		   TwistStamped, Imu). To add a new one, simply append the next number in the sequence to its base name,
@@ -67,6 +67,7 @@
 		   parameters obviously have no default values, and must be specified. -->
 		<param name="odom0" value="odom"/>
 		<param name="imu0" value="imu"/>
+		<rosparam param="imu0_queue_size">10</rosparam>
 
 		<!-- Each sensor reading updates some or all of the filter's state. These options give you greater control over
 		   which values from each measurement are fed to the filter. For example, if you have an odometry message as input,
@@ -77,8 +78,8 @@
 		   making this parameter required for each sensor. -->
 		<!-- x/y not included because of redundancy with velocities -->
 		<!-- vyaw not included in odom because too inaccurate -->
-		<rosparam param="odom0_config">[false, false, false, false, false, false, true, true, false, false, false, false, false, false, false]</rosparam>
-		<rosparam param="imu0_config">[false, false, false, false, false, true, false, false, false, false, false, true, false, false, false]</rosparam>
+		<rosparam param="odom0_config">[false, false, false, false, false, false, true, true, true, false, false, true, false, false, false]</rosparam>
+		<rosparam param="imu0_config">[false, false, false, true, true, true, false, false, false, false, false, false, false, false, false]</rosparam>
 
 		<!-- The best practice for including new sensors in robot_localization's state estimation nodes is to pass in velocity
 		   measurements and let the nodes integrate them. However, this isn't always feasible, and so the state estimation
@@ -114,6 +115,10 @@
 		   if the node is unhappy with any settings or data. -->
 		<param name="print_diagnostics" value="true"/>
 
+		<!-- If true, will dynamically scale the process_noise_covariance based on the robot?s velocity. This is useful, e.g., when you want your
+		     robot?s estimate error covariance to stop growing when the robot is stationary. Defaults to false. -->
+		<param name="dynamic_process_noise_covariance" value="true"/>
+
 		<remap from="odometry/filtered" to="odom_combined"/>
 	</node>
 </launch>
diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index 9f2c287..a3f39f9 100644
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -15,7 +15,7 @@
 		<rosparam command="load" file="$(find wild_thumper)/config/analyzers.yaml"/>
 	</node>
 
-	<node pkg="wild_thumper" type="wt_node.py" name="wild_thumper" output="screen" respawn="false" required="false">
+	<node pkg="wild_thumper" type="wt_node.py" name="wild_thumper" output="screen" respawn="false" required="true">
 		<param name="enable_odom_tf" value="true" unless="$(arg use_imu)"/>
 		<param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
 	</node>
@@ -33,6 +33,7 @@
 		<arg name="nodelet_manager_name" value="nodelet_manager" />
 	</include>
 
+	<!--
 	<node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf" output="screen" if="$(arg use_imu)">
 		<remap from="imu_data" to="imu"/>
 		<remap from="vo" to="gps"/>
@@ -49,8 +50,8 @@
 		<param name="frame_id" value="odom"/>
 		<param name="child_frame_id" value="base_footprint"/>
 	</node>
+	-->
 
-	<!--
 	<include file="$(find wild_thumper)/launch/robot_localization.launch" if="$(arg use_imu)"/>
 	<node pkg="robot_localization" type="navsat_transform_node" name="navsat_transform_node">
 		<param name="magnetic_declination_radians" value="0.48"/>
@@ -63,5 +64,4 @@
 		<remap from="/odometry/filtered" to="odom_combined"/>
 		<remap from="/odometry/gps" to="gps"/>
 	</node>
-	-->
 </launch>
diff --git a/scripts/sensor_board.py b/scripts/sensor_board.py
index 8efeb16..e431b57 100755
--- a/scripts/sensor_board.py
+++ b/scripts/sensor_board.py
@@ -11,8 +11,8 @@ from pyshared.humidity import *
 from wild_thumper.msg import Sensor
 
 # Board warming offset
-TEMP_ERROR = -5 # degree celsius
-PRESSURE_ERROR = -4.5
+TEMP_ERROR = 0 # -5 # degree celsius
+PRESSURE_ERROR = -2.5
 
 """
 LDR:
diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 1549ea6..1cf9d8d 100755
--- a/scripts/wt_node.py
+++ b/scripts/wt_node.py
@@ -237,9 +237,9 @@ class MoveBase:
 		odom.pose.pose.orientation.w = odom_quat[3]
 		odom.pose.covariance[0] = self.odom_covar_xy # x
 		odom.pose.covariance[7] = self.odom_covar_xy # y
-		odom.pose.covariance[14] = 99999 # z
-		odom.pose.covariance[21] = 99999 # rotation about X axis
-		odom.pose.covariance[28] = 99999 # rotation about Y axis
+		odom.pose.covariance[14] = self.odom_covar_xy # z
+		odom.pose.covariance[21] = self.odom_covar_angle # rotation about X axis
+		odom.pose.covariance[28] = self.odom_covar_angle # rotation about Y axis
 		odom.pose.covariance[35] = self.odom_covar_angle # rotation about Z axis
 
 		# set the velocity
diff --git a/urdf/wild_thumper.urdf.xacro b/urdf/wild_thumper.urdf.xacro
index 47039b6..8be843c 100644
--- a/urdf/wild_thumper.urdf.xacro
+++ b/urdf/wild_thumper.urdf.xacro
@@ -137,19 +137,19 @@
 	<joint name="sonar_forward_left_joint" type="fixed">
 		<parent link="mounting_plate"/>
 		<child link="sonar_forward_left"/>
-		<origin xyz="0.115 0.05 -0.012" rpy="0 0 0"/>
+		<origin xyz="0.115 0.05 -0.012" rpy="0 ${-5*PI/180} 0"/>
 	</joint>
 
 	<joint name="sonar_forward_right_joint" type="fixed">
 		<parent link="mounting_plate"/>
 		<child link="sonar_forward_right"/>
-		<origin xyz="0.115 -0.05 -0.012" rpy="0 0 0"/>
+		<origin xyz="0.115 -0.05 -0.012" rpy="0 ${-5*PI/180} 0"/>
 	</joint>
 
 	<joint name="sonar_backward_joint" type="fixed">
 		<parent link="mounting_plate"/>
 		<child link="sonar_backward"/>
-		<origin xyz="-0.115 0.0 -0.012" rpy="0 ${PI} 0"/>
+		<origin xyz="-0.115 0.0 -0.012" rpy="0 ${-175*PI/180} 0"/>
 	</joint>
 
 	<joint name="ir_left_joint" type="fixed">
-- 
2.39.2