From 10405f7578977ead223bda080bb10e73839d1219 Mon Sep 17 00:00:00 2001 From: Erik Andresen Date: Mon, 27 Jul 2020 21:57:07 +0200 Subject: [PATCH] implement and use tinkerforge_imu2 node in cpp --- .screen-startup | 4 +- CMakeLists.txt | 6 ++- config/robot_localization.yaml | 20 ++++++++ launch/3dsensor.launch | 9 ++-- launch/wild_thumper.launch | 2 +- scripts/sensor_board.py | 22 ++++++--- src/tinkerforge_imu2.cpp | 88 ++++++++++++++++++++++++++++++++++ 7 files changed, 138 insertions(+), 13 deletions(-) create mode 100644 src/tinkerforge_imu2.cpp diff --git a/.screen-startup b/.screen-startup index 9817cb9..28799d9 100644 --- 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 be7eeab..3703bb7 100644 --- 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 bce9d74..4049867 100644 --- 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 8f4bac1..f09bcba 100644 --- a/launch/3dsensor.launch +++ b/launch/3dsensor.launch @@ -3,14 +3,17 @@ - - + + - + + + diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch index e6b426c..c0f71b8 100644 --- a/launch/wild_thumper.launch +++ b/launch/wild_thumper.launch @@ -27,7 +27,7 @@ - + diff --git a/scripts/sensor_board.py b/scripts/sensor_board.py index c688ff9..c5e84c4 100755 --- 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 index 0000000..d07b8e7 --- /dev/null +++ b/src/tinkerforge_imu2.cpp @@ -0,0 +1,88 @@ +#include +#include +#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("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 +} -- 2.39.2