From: Erik Andresen <erik@vontaene.de>
Date: Tue, 28 Jul 2020 08:03:14 +0000 (+0200)
Subject: tinkerforge imu: switch node to tinkerforge_imu_ros
X-Git-Url: https://defiant.homedns.org/gitweb/?p=ros_wild_thumper.git;a=commitdiff_plain;h=15e32d80e6d095cc1d5550dc340be241cf6badcc

tinkerforge imu: switch node to tinkerforge_imu_ros
---

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3703bb7..5dfe0e2 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -109,7 +109,6 @@ 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,10 +119,6 @@ target_link_libraries(path_following
    ${catkin_LIBRARIES}
    ${OpenCV_LIBS}
 )
-target_link_libraries(imu_brick_v2
-   ${catkin_LIBRARIES}
-   tinkerforge
-)
 
 #############
 ## Install ##
diff --git a/launch/wild_thumper.launch b/launch/wild_thumper.launch
index c0f71b8..1e1bf66 100644
--- a/launch/wild_thumper.launch
+++ b/launch/wild_thumper.launch
@@ -27,8 +27,15 @@
 		<param name="enable_odom_tf" value="false" if="$(arg use_imu)"/>
 	</node>
 
-	<node pkg="wild_thumper" type="imu_brick_v2" name="tinkerforge_imu_brick2" output="screen" respawn="true" if="$(arg use_imu)">
+	<node pkg="tinkerforge_imu_ros" type="tinkerforge_imu_ros" name="tinkerforge_imu_brick2" output="screen" respawn="true" if="$(arg use_imu)">
 		<param name="uid" value="6DdNSn"/>
+		<param name="frame_id" value="base_imu_link"/>
+		<param name="period_ms" value="20"/>
+		<!-- 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 -->
+		<param name="variance_orientation" value="0.008"/>
+		<remap from="/tinkerforge_imu_brick2/imu" to="imu"/>
 	</node>
 
 	<include file="$(find yocs_cmd_vel_mux)/launch/cmd_vel_mux.launch">
diff --git a/package.xml b/package.xml
index 0c68507..773517e 100644
--- a/package.xml
+++ b/package.xml
@@ -56,6 +56,7 @@
   <run_depend>rosruby</run_depend>
   <run_depend>amcl</run_depend>
   <run_depend>gmapping</run_depend>
+  <run_depend>tinkerforge_imu_ros</run_depend>
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
diff --git a/src/tinkerforge_imu2.cpp b/src/tinkerforge_imu2.cpp
deleted file mode 100644
index d07b8e7..0000000
--- a/src/tinkerforge_imu2.cpp
+++ /dev/null
@@ -1,88 +0,0 @@
-#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
-}