implement and use tinkerforge_imu2 node in cpp

diff --git a/.screen-startup b/.screen-startup
index 9817cb918c35e8d5ac0ff54e4b7b580c7aee7b7f..28799d90a4f3d917bc495d2d0ece1027665ca61b 100644 (file)
--- a/.screen-startup
+++ b/.screen-startup
@@ -3,8 +3,10 @@
 
 source $HOME/.screenrc
 
-screen 0 zsh -is eval 'roscore'
+screen 0 zsh -is eval 'sleep 10; roscore'
 screen 1 zsh -is eval 'roslaunch --wait wild_thumper wild_thumper.launch'
 screen 2 zsh -is eval '(($(/usr/sbin/i2cget -y 2 0x38 || echo 0xff) & 0x01)) || roslaunch --wait wild_thumper teleop.launch'
+#screen 3 zsh -is eval '(($(/usr/sbin/i2cget -y 2 0x38 || echo 0xff) & 0x02)) || roslaunch --wait wild_thumper move_base.launch'
+#screen 4 zsh -is eval '(($(/usr/sbin/i2cget -y 2 0x38 || echo 0xff) & 0x02)) || (sleep 20 && rosservice call global_localization)'
 #screen 3 zsh -is eval 'roslaunch --wait wild_thumper move_base.launch nomap:=true'
 select 2

diff --git a/CMakeLists.txt b/CMakeLists.txt
index be7eeab0689a717392fe886a6360e39f8d736078..3703bb7978f47271e67aec76a14b4ae4e87e7714 100644 (file)
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -100,7 +100,6 @@ catkin_package(
 ## Your package locations should be listed before other locations
 include_directories(
     ${catkin_INCLUDE_DIRS}
-    /opt/ros/tinkerforge/c_bindings/source
 )
 
 ## Declare a cpp library
@@ -110,6 +109,7 @@ include_directories(
 
 ## Declare a cpp executable
 add_executable(path_following src/path_following.cpp)
+add_executable(imu_brick_v2 src/tinkerforge_imu2.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes
@@ -120,6 +120,10 @@ target_link_libraries(path_following
    ${catkin_LIBRARIES}
    ${OpenCV_LIBS}
 )
+target_link_libraries(imu_brick_v2
+   ${catkin_LIBRARIES}
+   tinkerforge
+)
 
 #############
 ## Install ##

diff --git a/config/robot_localization.yaml b/config/robot_localization.yaml
index bce9d7466b012d5ed373181739ef0f08ae2dc5a7..404986785e40370908419e4dec5ce26d7f7b884e 100644 (file)
--- a/config/robot_localization.yaml
+++ b/config/robot_localization.yaml
@@ -146,3 +146,23 @@ process_noise_covariance: [2.5,  0,    0,    0,    0,    0,    0,     0,     0,
                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0.01, 0,    0,
                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0.01, 0,
                            0,    0,    0,    0,    0,    0,    0,     0,     0,    0,    0,    0,    0,    0,    0.015]
+
+#initial_estimate_covariance: [0.0082783,   0.00015023,  0,           0,           0,           1.4766e-22,  0.0056327,   -0.0038416,  0,           0,           0,           1.863e-22,   0.0031683,   -0.0021608,  0,
+#                              0.00015023,  0.010788,    0,           0,           0,           -3.5314e-22, 0.0038418,   0.0056329,   0,           0,           0,           -4.4555e-22, 0.0021611,   0.0031687,   0,
+#                              0,           0,           0.0018072,   3.2715e-24,  1.5934e-23,  0,           0,           0,           0.0054835,   8.4669e-24,  4.1238e-23,  0,           0,           0,           0.00033362,
+#                              0,           0,           3.2715e-24,  0.00044495,  -6.2008e-49, 0,           0,           0,           -1.2846e-33, 0.0013386,   -7.0886e-35, 0,           0,           0,           -4.1097e-35,
+#                              0,           0,           1.5934e-23,  -6.2142e-49, 0.00044495,  0,           0,           0,           -6.2545e-33, 7.0886e-35,  0.0013386,   0,           0,           0,           -2.0009e-34,
+#                              1.4766e-22,  -3.5314e-22, 0,           0,           0,           0.006296,    -2.6304e-31, 2.1336e-30,  0,           0,           0,           0.0091462,   -4.0833e-31, 3.3122e-30,  0,
+#                              0.0056327,   0.0038418,   0,           0,           0,           -2.6304e-31, 0.023382,    1.2088e-09,  0,           0,           0,           -1.8639e-30, 0.011984,    1.8766e-09,  0,
+#                              -0.0038416,  0.0056329,   0,           0,           0,           2.1336e-30,  1.2088e-09,  0.023382,    0,           0,           0,           1.3775e-29,  1.8766e-09,  0.011984,    0,
+#                              0,           0,           0.0054835,   -1.2846e-33, -6.2545e-33, 0,           0,           0,           0.023656,    1.265e-37,   6.1588e-37,  0,           0,           0,           0.0020076,
+#                              0,           0,           8.4669e-24,  0.0013386,   7.0886e-35,  0,           0,           0,           1.265e-37,   0.005748,    -6.135e-57,  0,           0,           0,           4.0468e-39,
+#                              0,           0,           4.1238e-23,  -7.0886e-35, 0.0013386,   0,           0,           0,           6.1588e-37,  -6.135e-57,  0.005748,    0,           0,           0,           1.9703e-38,
+#                              1.863e-22,   -4.4555e-22, 0,           0,           0,           0.0091462,   -1.8639e-30, 1.3775e-29,  0,           0,           0,           0.01982,     -2.8935e-30, 2.1383e-29,  0,           
+#                              0.0031683,   0.0021611,   0,           0,           0,           -4.0833e-31, 0.011984,    1.8766e-09,  0,           0,           0,           -2.8935e-30, 0.022337,    2.9131e-09,  0,
+#                              -0.0021608,  0.0031687,   0,           0,           0,           3.3122e-30,  1.8766e-09,  0.011984,    0,           0,           0,           2.1383e-29,  2.9131e-09,  0.022337,    0,           
+#                              0,           0,           0.00033362,  -4.1097e-35, -2.0009e-34, 0,           0,           0,           0.0020076,   4.0468e-39,  1.9703e-38,  0,           0,           0,           0.0086215]
+#
+#
+#debug: true
+#debug_out_file: "/tmp/log.txt"

diff --git a/launch/3dsensor.launch b/launch/3dsensor.launch
index 8f4bac1a9d5acd37c374dd4790788c7304cb4bbc..f09bcba81dd1faa02d22b9b9a629d1afd39c8aa9 100644 (file)
--- a/launch/3dsensor.launch
+++ b/launch/3dsensor.launch
@@ -3,14 +3,17 @@
        <include file="$(find openni2_launch)/launch/openni2.launch">
                <arg name="depth_registration" value="true" />
        </include>
-       <!-- Depth: QQVGA 30Hz -->
-       <param name="/camera/driver/depth_mode" value="11" />
+       <!-- Depth: QVGA 30Hz -->
+       <param name="/camera/driver/depth_mode" value="8" />
        <!-- Color: QVGA 30Hz -->
        <param name="/camera/driver/color_mode" value="8" />
-       <param name="/camera/driver/data_skip" value="1" />
+       <!-- reduce to 5Hz -->
+       <param name="/camera/driver/data_skip" value="5" />
 
+       <!--
        <node pkg="nodelet" type="nodelet" name="depthimage_to_laserscan" args="load depthimage_to_laserscan/DepthImageToLaserScanNodelet /camera/camera_nodelet_manager" output="screen">
                <remap from="/camera/image" to="/camera/depth/image"/>
                <remap from="/camera/scan" to="/scan"/>
        </node>
+       -->
 </launch>

diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index e6b426cb88290f11831b36b40b8f35257b80f7a3..c0f71b8117b1df668cac5c2e136782c6f8671720 100644 (file)
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -27,7 +27,7 @@
                <param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
        </node>
 
-       <node pkg="wild_thumper" type="tinkerforge_imu2.py" name="tinkerforge_imu_brick2" output="screen" respawn="true" if="$(arg use_imu)">
+       <node pkg="wild_thumper" type="imu_brick_v2" name="tinkerforge_imu_brick2" output="screen" respawn="true" if="$(arg use_imu)">
                <param name="uid" value="6DdNSn"/>
        </node>
 

diff --git a/scripts/sensor_board.py b/scripts/sensor_board.py
index c688ff9616c019d5f7b5c5c3b30cb1d8a54a6d3f..c5e84c45bca6a1acbddd784137217bf720443ee1 100755 (executable)
--- a/scripts/sensor_board.py
+++ b/scripts/sensor_board.py
@@ -5,22 +5,26 @@ import sys
 import rospy
 import struct
 import prctl
+import logging
 from datetime import datetime
 from time import sleep
+from math import *
 from pyshared.i2c import i2c
 from pyshared.humidity import *
 from wild_thumper.msg import Sensor
 
 # Board warming offset
-TEMP_ERROR = 0 # -2.5 # -5 # degree celsius
+TEMP_ERROR = -3.0 # -5 # degree celsius
 PRESSURE_ERROR = -2.5
 
 """
 LDR:
-val = [100 285 321 515 636 758 940 1023]
-lux = [95  34  31  12  11   8    5    0]
+val = [1023, 396.8, 110.75, 28.1]
+lux = [0, 13.8, 164.9, 1341]
 """
 
+logger = logging.getLogger(__name__)
+
 def get_i2c(addr):
        dev = i2c(addr)
        dev.write(struct.pack(">BB", 0x00, 0x0f))
@@ -39,7 +43,7 @@ def get(addr=0x58):
 
        temp_mess/=10.0
        humidity_mess/=10.0
-       lux = -3.9338e-07*ldr**3 +0.00083596*ldr**2 -0.58608*ldr +144.96
+       lux = exp(-8.1065e-08*ldr**3 +8.8678e-05*ldr**2 -0.03636*ldr +8.1547)
        pressure_v = pressure*2.56/1024
        pressure_kpa = P = (pressure_v/5 + 0.04) / 0.004 + PRESSURE_ERROR # datasheet
 
@@ -73,9 +77,13 @@ class SensorBoard:
                while not rospy.is_shutdown():
                        if self.pub.get_num_connections() > 0:
                                ldr, temp, humidity, pressure, co = get()
-                               if (datetime.now() - t_last_check).seconds > 900:
-                                       ventilate = check_ventilate(temp, humidity)
-                                       t_last_check = datetime.now()
+                               ventilate = None
+                               try:
+                                       if (datetime.now() - t_last_check).seconds > 900:
+                                               ventilate = check_ventilate(temp, humidity)
+                                               t_last_check = datetime.now()
+                               except Exception as e:
+                                       logger.error(e)
                                
                                msg = Sensor()
                                msg.header.stamp = rospy.Time.now()

diff --git a/src/tinkerforge_imu2.cpp b/src/tinkerforge_imu2.cpp
new file mode 100644 (file)
index 0000000..d07b8e7
--- /dev/null
+++ b/src/tinkerforge_imu2.cpp
@@ -0,0 +1,88 @@
+#include <stdio.h>
+#include <math.h>
+#include "ros/ros.h"
+#include "sensor_msgs/Imu.h"
+#include "ip_connection.h"
+#include "brick_imu_v2.h"
+
+ros::Publisher pub_imu;
+
+void cb_all_data(int16_t acceleration[3], int16_t magnetic_field[3], int16_t angular_velocity[3], int16_t euler_angle[3], int16_t quaternion[4], int16_t linear_acceleration[3], int16_t gravity_vector[3], int8_t temperature, uint8_t calibration_status, void *user_data) {
+       sensor_msgs::Imu msg;
+       msg.header.stamp = ros::Time::now();
+       msg.header.frame_id = "base_imu_link";
+
+       msg.orientation.x = quaternion[1]/16383.0;
+       msg.orientation.y = quaternion[2]/16383.0;
+       msg.orientation.z = quaternion[3]/16383.0;
+       msg.orientation.w = quaternion[0]/16383.0;
+       // Observed orientation variance: 0.0 (10k samples)
+       // Magnometer heading accuracy is +-2.5 deg => 0.088 rad
+       // With heading accuracy as std dev, variance = 0.088^2 = 0.008 
+       msg.orientation_covariance = {
+               0.008, 0     , 0,
+               0    , 0.008, 0,
+               0    , 0    , 0.008
+       };
+
+       msg.angular_velocity.x = angular_velocity[0]/16.0*M_PI/180;
+       msg.angular_velocity.y = angular_velocity[1]/16.0*M_PI/180;
+       msg.angular_velocity.z = angular_velocity[2]/16.0*M_PI/180;
+       // Observed angular velocity variance: 0.006223 (10k samples), => round up to 0.02
+       msg.angular_velocity_covariance = {
+               0.02, 0   , 0,
+               0   , 0.02, 0,
+               0   , 0   , 0.02
+       };
+
+       msg.linear_acceleration.x = acceleration[0]/100.0;
+       msg.linear_acceleration.y = acceleration[1]/100.0;
+       msg.linear_acceleration.z = acceleration[2]/100.0;
+       // Observed linear acceleration variance: 0.001532 (10k samples)
+       // Calculation for variance taken from razor imu:
+       // nonliniarity spec: 1% of full scale (+-2G) => 0.2m/s^2
+       // Choosing 0.2 as std dev, variance = 0.2^2 = 0.04
+       msg.linear_acceleration_covariance = {
+               0.04, 0    , 0,
+               0    , 0.04, 0,
+               0    , 0    , 0.04
+       };
+
+       pub_imu.publish(msg);
+}
+
+int main(int argc, char **argv) {
+       std::string host, uid;
+       int port;
+       IPConnection ipcon;
+       IMUV2 imu;
+
+       ros::init(argc, argv, "imu_brick_v2");
+       ros::NodeHandle nh;
+       ros::NodeHandle pnh("~");
+
+       pub_imu = nh.advertise<sensor_msgs::Imu>("imu", 10);
+
+       pnh.param("host", host, std::string("localhost"));
+       pnh.param("port", port, 4223);
+       pnh.getParam("uid", uid);
+
+       // Create IP connection
+       ipcon_create(&ipcon);
+       // Create device object
+       imu_v2_create(&imu, uid.c_str(), &ipcon);
+       // Connect to brickd
+       if(ipcon_connect(&ipcon, host.c_str(), port) < 0) {
+               fprintf(stderr, "Could not connect\n");
+               return 1;
+       }
+       imu_v2_leds_off(&imu);
+       imu_v2_disable_status_led(&imu);
+       imu_v2_register_callback(&imu, IMU_V2_CALLBACK_ALL_DATA, (void (*)(void))cb_all_data, NULL);
+       imu_v2_set_all_data_period(&imu, 20); // ms
+
+       ros::spin();
+
+       imu_v2_destroy(&imu);
+       ipcon_destroy(&ipcon); // Calls ipcon_disconnect internally
+}