diff --git a/rb_ws/src/buggy/buggy/buggy_state_converter.py b/rb_ws/src/buggy/buggy/buggy_state_converter.py index cb8fc1c..f3dd023 100644 --- a/rb_ws/src/buggy/buggy/buggy_state_converter.py +++ b/rb_ws/src/buggy/buggy/buggy_state_converter.py @@ -74,13 +74,14 @@ def convert_SC_state(self, msg): yaw = self.quaternion_to_yaw(qx, qy, qz, qw) converted_msg.pose.pose.orientation.x = yaw # ignored: - # converted_msg.pose.pose.orientation.y = qy - # converted_msg.pose.pose.orientation.z = qz - # converted_msg.pose.pose.orientation.w = qw + converted_msg.pose.pose.orientation.y = 0.0 + converted_msg.pose.pose.orientation.z = 0.0 + converted_msg.pose.pose.orientation.w = 0.0 # ---- 3. Copy Covariances (Unchanged) ---- - converted_msg.pose.covariance = msg.pose.covariance - converted_msg.twist.covariance = msg.twist.covariance + # TODO: check whether covariances should be copied over or not sent at all + # converted_msg.pose.covariance = msg.pose.covariance + # converted_msg.twist.covariance = msg.twist.covariance # ---- 4. Copy Linear Velocities ---- converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction @@ -107,16 +108,17 @@ def convert_NAND_state(self, msg): # ---- 2. Orientation in Radians with 0 at East ---- converted_msg.pose.pose.orientation.x = msg.pose.pose.orientation.x # ignored: - # converted_msg.pose.pose.orientation.y = msg.pose.pose.orientation.y - # converted_msg.pose.pose.orientation.z = msg.pose.pose.orientation.z - # converted_msg.pose.pose.orientation.w = msg.pose.pose.orientation.w + converted_msg.pose.pose.orientation.y = 0.0 + converted_msg.pose.pose.orientation.z = 0.0 + converted_msg.pose.pose.orientation.w = 0.0 # ---- 3. Copy Covariances (Unchanged) ---- - converted_msg.pose.covariance = msg.pose.covariance - converted_msg.twist.covariance = msg.twist.covariance + # TODO: check whether covariances should be copied over or not sent at all + # converted_msg.pose.covariance = msg.pose.covariance + # converted_msg.twist.covariance = msg.twist.covariance # ---- 4. Copy Linear Velocities ---- - # CHECK: ROS serial translator node must change scalar speed to velocity x/y components before pushing to raw_state + # TODO: check that ROS serial translator node changes scalar speed to velocity x/y components before pushing to raw_state converted_msg.twist.twist.linear.x = msg.twist.twist.linear.x # m/s in x-direction converted_msg.twist.twist.linear.y = msg.twist.twist.linear.y # m/s in y-direction converted_msg.twist.twist.linear.z = msg.twist.twist.linear.z # Keep original Z velocity (??) @@ -131,7 +133,8 @@ def convert_NAND_state(self, msg): def quaternion_to_yaw(self, qx, qy, qz, qw): """Convert a quaternion to yaw (heading) in radians.""" siny_cosp = 2.0 * (qw * qz + qx * qy) - cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) + cosy_cosp = 1.0 - 2.0 * (qy * qy + qz * qz) # assumes quaternion is normalized, should be true in ROS, + # else use (qw * qw + qx * qx - qy * qy - qz * qz) return np.arctan2(siny_cosp, cosy_cosp)