Fix RPY angles direction.

diff --git a/src/bno055_node.cpp b/src/bno055_node.cpp
index f44d816..b8471d1 100644 (file)
--- a/src/bno055_node.cpp
+++ b/src/bno055_node.cpp
@@ -270,10 +270,11 @@ void Bno055Node::sample(void)
 
        struct bno055_euler_t euler;
        if(bno055_read_euler_hrp(&euler) == BNO055_SUCCESS)
-                rpy_ << euler.r/BNO055_EULER_DIV_RAD,
-                euler.p/BNO055_EULER_DIV_RAD,
+                rpy_ << -euler.p/BNO055_EULER_DIV_RAD,
+                -euler.r/BNO055_EULER_DIV_RAD,
                 euler.h/BNO055_EULER_DIV_RAD;
         else RCLCPP_ERROR_STREAM(get_logger(),"Error reading Euler angles.\n");
+               rpy_[2]=atan2(sin(-rpy_[2]),cos(rpy_[2]));
 
        s8 temp;
        if(bno055_read_temp_data(&temp) == BNO055_SUCCESS)