From b60619c67db267cd80230f9f03dd74e284f92ae9 Mon Sep 17 00:00:00 2001
From: seiya0412 <seiya.shimizu@whill.inc>
Date: Sat, 18 Sep 2021 22:30:41 +0900
Subject: [PATCH 1/2] remove imu data

---
 src/ros_whill.cpp          | 19 -------------------
 src/whill/PacketParser.cpp |  8 --------
 src/whill/WHILL.h          |  9 ---------
 3 files changed, 36 deletions(-)

diff --git a/src/ros_whill.cpp b/src/ros_whill.cpp
index b4db2a8..5f5b594 100644
--- a/src/ros_whill.cpp
+++ b/src/ros_whill.cpp
@@ -32,7 +32,6 @@ SOFTWARE.
 #include "ros/ros.h"
 #include "sensor_msgs/Joy.h"
 #include "sensor_msgs/JointState.h"
-#include "sensor_msgs/Imu.h"
 #include "sensor_msgs/BatteryState.h"
 #include "nav_msgs/Odometry.h"
 
@@ -78,7 +77,6 @@ void safeDelete(T *&p)
 // Publishers
 ros::Publisher ros_joystick_state_publisher;
 ros::Publisher ros_jointstate_publisher;
-ros::Publisher ros_imu_publisher;
 ros::Publisher ros_battery_state_publisher;
 ros::Publisher ros_odom_publisher;
 
@@ -297,22 +295,6 @@ void whill_callback_data1(WHILL *caller)
     joy.axes[1] = caller->joy.y / 100.0f;  //Y
     ros_joystick_state_publisher.publish(joy);
 
-    // IMU
-    sensor_msgs::Imu imu;
-    imu.header.stamp = currentTime;
-    imu.header.frame_id = "imu";
-
-    imu.orientation_covariance[0] = -1; // Orientation is unknown
-
-    imu.angular_velocity.x = caller->gyro.x / 180 * M_PI; // deg per sec to rad/s
-    imu.angular_velocity.y = caller->gyro.y / 180 * M_PI; // deg per sec to rad/s
-    imu.angular_velocity.z = caller->gyro.z / 180 * M_PI; // deg per sec to rad/s
-
-    imu.linear_acceleration.x = caller->accelerometer.x * 9.80665; // G to m/ss
-    imu.linear_acceleration.y = caller->accelerometer.y * 9.80665; // G to m/ssnav_msgs::Odometry odom_msg = odom.getROSOdometry();
-    imu.linear_acceleration.z = caller->accelerometer.z * 9.80665; // G to m/ss
-    ros_imu_publisher.publish(imu);
-
     // Battery
     sensor_msgs::BatteryState batteryState;
     batteryState.header.stamp = currentTime;
@@ -430,7 +412,6 @@ int main(int argc, char **argv)
     // Publishers
     ros_joystick_state_publisher = nh.advertise<sensor_msgs::Joy>("states/joy", 100);
     ros_jointstate_publisher = nh.advertise<sensor_msgs::JointState>("states/jointState", 100);
-    ros_imu_publisher = nh.advertise<sensor_msgs::Imu>("states/imu", 100);
     ros_battery_state_publisher = nh.advertise<sensor_msgs::BatteryState>("states/batteryState", 100);
     ros_odom_publisher = nh.advertise<nav_msgs::Odometry>("odom", 100);
 
diff --git a/src/whill/PacketParser.cpp b/src/whill/PacketParser.cpp
index 4e47f69..2ea60a2 100644
--- a/src/whill/PacketParser.cpp
+++ b/src/whill/PacketParser.cpp
@@ -63,14 +63,6 @@ void WHILL::PacketParser::parseDataset0(Packet* packet){
 }
 
 void WHILL::PacketParser::parseDataset1(Packet* packet){
-  whill->accelerometer.x = (signed short)((packet->payload[1] << 8)+(packet->payload[2]));
-  whill->accelerometer.y = (signed short)((packet->payload[3] << 8)+(packet->payload[4]));
-  whill->accelerometer.z = (signed short)((packet->payload[5] << 8)+(packet->payload[6]));
-
-  whill->gyro.x = (signed short)((packet->payload[7] << 8)+(packet->payload[8]));
-  whill->gyro.y = (signed short)((packet->payload[9] << 8)+(packet->payload[10]));
-  whill->gyro.z = (signed short)((packet->payload[11] << 8)+(packet->payload[12]));
-
   whill->joy.y = (int)(signed char)packet->payload[13];
   whill->joy.x = (int)(signed char)packet->payload[14];
 
diff --git a/src/whill/WHILL.h b/src/whill/WHILL.h
index 13903ab..0f3846b 100644
--- a/src/whill/WHILL.h
+++ b/src/whill/WHILL.h
@@ -172,13 +172,6 @@ class WHILL
         Pack turn;
     };
 
-    typedef struct
-    {
-        int x;
-        int y;
-        int z;
-    } Data3D;
-
     typedef struct
     {
         int x;
@@ -197,8 +190,6 @@ class WHILL
         int speed;
     } Motor;
 
-    Data3D accelerometer = {0};
-    Data3D gyro = {0};
     Joy virtual_joy = {0};
     Joy joy = {0};
     Battery battery = {0};

From e6931ca551e2da4629422ef00cdb499ff2957c5c Mon Sep 17 00:00:00 2001
From: seiya0412 <seiya.shimizu@whill.inc>
Date: Sat, 18 Sep 2021 22:31:29 +0900
Subject: [PATCH 2/2] fix format

---
 src/whill/WHILL.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/whill/WHILL.cpp b/src/whill/WHILL.cpp
index a85e881..9b3e83e 100644
--- a/src/whill/WHILL.cpp
+++ b/src/whill/WHILL.cpp
@@ -63,7 +63,7 @@ void WHILL::receivePacket(){
 
 void WHILL::refresh(){
     // Scan the data from interface
-   receivePacket();
+    receivePacket();
 }