Fix inverted head tilt orientation

AndrejOrsula · Apr 5, 2020 · 89216ba · 89216ba
1 parent e9df952
commit 89216ba
Show file tree

Hide file tree

Showing 2 changed files with 2 additions and 2 deletions.
diff --git a/ros2_openface/src/openface.cpp b/ros2_openface/src/openface.cpp
@@ -311,7 +311,7 @@ void Ros2OpenFace::image_callback(const sensor_msgs::msg::Image::ConstSharedPtr
       face.face.head_pose.position.y = head_pose[1] / 1000.0;
       face.face.head_pose.position.z = head_pose[2] / 1000.0;
       tf2::Quaternion head_orientation;
-      head_orientation.setRPY(-head_pose[3], head_pose[4] + M_PI, -head_pose[5]);
+      head_orientation.setRPY(-head_pose[3], head_pose[4] + M_PI, head_pose[5]);
       face.face.head_pose.orientation = tf2::toMsg(head_orientation);
 
       if (this->get_parameter("broadcast_tf").get_value<bool>())

diff --git a/ros2_openface/src/openface_separate.cpp b/ros2_openface/src/openface_separate.cpp
@@ -394,7 +394,7 @@ void Ros2OpenFaceSeparate::image_callback(const sensor_msgs::msg::Image::ConstSh
       msg_head_pose.pose.position.y = head_pose[1] / 1000.0;
       msg_head_pose.pose.position.z = head_pose[2] / 1000.0;
       tf2::Quaternion head_orientation;
-      head_orientation.setRPY(-head_pose[3], head_pose[4] + M_PI, -head_pose[5]);
+      head_orientation.setRPY(-head_pose[3], head_pose[4] + M_PI, head_pose[5]);
       msg_head_pose.pose.orientation = tf2::toMsg(head_orientation);
 
       pub_head_pose_->publish(msg_head_pose);