Set sonar angle to 5° degree

diff --git a/avr/motor_ctrl/main.c b/avr/motor_ctrl/main.c
index 16c8b0cc2d0c2a51d364d00b603270f79d58e20a..5d655290a7e8b78cb3efd33b881eaf85fafd3bfe 100644 (file)
--- 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
 #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 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
 
 #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
        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;
        }       
                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;
        }       
                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;
        }       
                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);
                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 ec2e8cc1cd2d2d7c00e6a94c2eb1e02557714e0b..aef12fea312558fcd42ff6c98387bc7a9f50a435 100644 (file)
--- a/avr/motor_ctrl/main.hex
+++ b/avr/motor_ctrl/main.hex
@@ -321,9 +321,9 @@
 :10140000B90AB501882777FD8095982FC0908E0086\r
 :10141000D0908F00E0909000F0909100C60ED71E03\r
 :10142000E81EF91EC0928E00D0928F00E0929000CC\r
 :10140000B90AB501882777FD8095982FC0908E0086\r
 :10141000D0908F00E0909000F0909100C60ED71E03\r
 :10142000E81EF91EC0928E00D0928F00E0929000CC\r

-:10143000F09291000E94FC1026EB33EF4DE75DE344\r
+:10143000F09291000E94FC102CEE31E548EB5DE348\r

 :101440000E94E9112B013C01C701B6010E94FC106A\r
 :101440000E94E9112B013C01C701B6010E94FC106A\r

-:1014500021E539E44DE95AE30E94E9119B01AC0111\r
+:1014500024E330E847E35AE30E94E9119B01AC0121\r

 :10146000C301B2010E94F60F6B017C0180918000E4\r
 :1014700090918100B501681B790B882777FD8095D5\r
 :10148000982F0E94FC1020E030E0A9010E94E91191\r
 :10146000C301B2010E94F60F6B017C0180918000E4\r
 :1014700090918100B501681B790B882777FD8095D5\r
 :10148000982F0E94FC1020E030E0A9010E94E91191\r


@@ -349,9 +349,9 @@
 :1015C000B90AB501882777FD8095982FC0908A00C9\r
 :1015D000D0908B00E0908C00F0908D00C60ED71E4E\r
 :1015E000E81EF91EC0928A00D0928B00E0928C0017\r
 :1015C000B90AB501882777FD8095982FC0908A00C9\r
 :1015D000D0908B00E0908C00F0908D00C60ED71E4E\r
 :1015E000E81EF91EC0928A00D0928B00E0928C0017\r

-:1015F000F0928D000E94FC1026EB33EF4DE75DE387\r
+:1015F000F0928D000E94FC102CEE31E548EB5DE38B\r

 :101600000E94E9112B013C01C701B6010E94FC10A8\r
 :101600000E94E9112B013C01C701B6010E94FC10A8\r

-:1016100021E539E44DE95AE30E94E9119B01AC014F\r
+:1016100024E330E847E35AE30E94E9119B01AC015F\r

 :10162000C301B2010E94F60F6B017C0180917E0024\r
 :1016300090917F00B501681B790B882777FD809515\r
 :10164000982F0E94FC1020E030E0A9010E94E911CF\r
 :10162000C301B2010E94F60F6B017C0180917E0024\r
 :1016300090917F00B501681B790B882777FD809515\r
 :10164000982F0E94FC1020E030E0A9010E94E911CF\r


@@ -377,9 +377,9 @@
 :10178000B90AB501882777FD8095982FC09086000B\r
 :10179000D0908700E0908800F0908900C60ED71E98\r
 :1017A000E81EF91EC0928600D0928700E092880061\r
 :10178000B90AB501882777FD8095982FC09086000B\r
 :10179000D0908700E0908800F0908900C60ED71E98\r
 :1017A000E81EF91EC0928600D0928700E092880061\r

-:1017B000F09289000E94FC1026EB33EF4DE75DE3C9\r
+:1017B000F09289000E94FC102CEE31E548EB5DE3CD\r

 :1017C0000E94E9112B013C01C701B6010E94FC10E7\r
 :1017C0000E94E9112B013C01C701B6010E94FC10E7\r

-:1017D00021E539E44DE95AE30E94E9119B01AC018E\r
+:1017D00024E330E847E35AE30E94E9119B01AC019E\r

 :1017E000C301B2010E94F60F6B017C0180917C0065\r
 :1017F00090917D00B501681B790B882777FD809556\r
 :10180000982F0E94FC1020E030E0A9010E94E9110D\r
 :1017E000C301B2010E94F60F6B017C0180917C0065\r
 :1017F00090917D00B501681B790B882777FD809556\r
 :10180000982F0E94FC1020E030E0A9010E94E9110D\r


@@ -405,9 +405,9 @@
 :10194000B90AB501882777FD8095982FC09082004D\r
 :10195000D0908300E0908400F0908500C60ED71EE2\r
 :10196000E81EF91EC0928200D0928300E0928400AB\r
 :10194000B90AB501882777FD8095982FC09082004D\r
 :10195000D0908300E0908400F0908500C60ED71EE2\r
 :10196000E81EF91EC0928200D0928300E0928400AB\r

-:10197000F09285000E94FC1026EB33EF4DE75DE30B\r
+:10197000F09285000E94FC102CEE31E548EB5DE30F\r

 :101980000E94E9112B013C01C701B6010E94FC1025\r
 :101980000E94E9112B013C01C701B6010E94FC1025\r

-:1019900021E539E44DE95AE30E94E9119B01AC01CC\r
+:1019900024E330E847E35AE30E94E9119B01AC01DC\r

 :1019A000C301B2010E94F60F6B017C0180917A00A5\r
 :1019B00090917B00B501681B790B882777FD809596\r
 :1019C000982F0E94FC1020E030E0A9010E94E9114C\r
 :1019A000C301B2010E94F60F6B017C0180917A00A5\r
 :1019B00090917B00B501681B790B882777FD809596\r
 :1019C000982F0E94FC1020E030E0A9010E94E9114C\r

diff --git a/config/wt_node.cfg b/config/wt_node.cfg
index 725c56519a73e35f89b248f0e0fa1ce020594eb2..aad0d5498c4007d96bb46234abf0021151690c78 100755 (executable)
--- 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("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)
 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 d3da3176d126d101ecd6ba882f80d79413ce5d0a..0a7aa6d7032237cf1f4038cc133b470a426debf5 100644 (file)
--- 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. -->
                   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
 
                <!-- 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. -->
                <!-- 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,
 
                <!-- 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"/>
                   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,
 
                <!-- 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 -->
                   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
 
                <!-- 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 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>
                <remap from="odometry/filtered" to="odom_combined"/>
        </node>
 </launch>

diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index 9f2c28769b1640e084175144c0e7357e6db2f1b0..a3f39f957fbb569c4c43cd44e81cbc34681037cf 100644 (file)
--- 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>
 
                <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>
                <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>
 
                <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"/>
        <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>
                <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"/>
        <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>
                <remap from="/odometry/filtered" to="odom_combined"/>
                <remap from="/odometry/gps" to="gps"/>
        </node>

-       -->

 </launch>
 </launch>

diff --git a/scripts/sensor_board.py b/scripts/sensor_board.py
index 8efeb162532a2c99664bc913732b3d9f25415052..e431b576a1569c34b4e3f178dd6b72e8fb91c2c8 100755 (executable)
--- 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
 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:
 
 """
 LDR:

diff --git a/scripts/wt_node.py b/scripts/wt_node.py
index 1549ea6f7aad56b906c7ad18321dd5857fadd882..1cf9d8d49fc42608ca1db0b1a0ed69873a95e4ef 100755 (executable)
--- 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.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
                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 47039b6dfca1512d1c9fdd3b5dae2ccdd1158762..8be843c4b6f126915e960f46af5d73aba252a708 100644 (file)
--- 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"/>
        <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"/>
        </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"/>
        </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">
        </joint>
 
        <joint name="ir_left_joint" type="fixed">