<commit_before># -*- coding:utf-8 -*-
"""
LookAt
^^^^^^

.. moduleauthor:: <NAME> <<EMAIL>>

This action moves the head to look at a given position on the floor relative to the robot.

The module expects a tuple containing the relative coordinates.
"""

import numpy as np
import rospy
import tf2_ros as tf2
from tf2_geometry_msgs import PointStamped
from geometry_msgs.msg import Point

from bitbots_stackmachine.abstract_action_module import AbstractActionModule


class AbstractLookAt(AbstractActionModule):
    def __init__(self, connector, args):
        super(AbstractLookAt, self).__init__(connector)
        self.tfBuffer = tf2.Buffer(cache_time=rospy.Duration(10.0))
        self.listener = tf2.TransformListener(self.tfBuffer)
        self.pan_speed_max = connector.config["Head"]["Search"]["maxPanSpeedSearch"]
        self.tilt_speed_max = connector.config["Head"]["Search"]["maxTiltSpeedSearch"]
        self.position_reached_time = rospy.get_time()

    def perform(self, connector, reevaluate=False):
        raise NotImplementedError

    def look_at(self, point, connector):
        """
        This method moves the head to a given point
        :param point: a PointStamped describing the point to look at
        """
        self.repr_data = {}

        try:
            point_camera = self.tfBuffer.transform(point, 'camera_fixed')
        except (tf2.LookupException, tf2.ConnectivityException):
            rospy.logdebug("Waiting for transform...")
            return

        # Calculate the head joint angles and clip them to the right range
        angle_tilt = np.rad2deg(np.arctan2(point_camera.point.z, point_camera.point.x))
        angle_tilt = np.clip(angle_tilt, connector.head.min_tilt, connector.head.max_tilt)
        angle_pan = np.rad2deg(np.arctan2(point_camera.point.y, point_camera.point.x))
        angle_pan = np.clip(angle_pan, connector.head.min_pan, connector.head.max_pan)

        current_pan_pos, current_tilt_pos = connector.head.get_current_head_pos()
        if (abs(current_pan_pos - angle_pan) < connector.head.delta and
                abs(current_tilt_pos - angle_tilt) < connector.head.delta):
            # We reached the position
            if rospy.get_time() - self.position_reached_time > connector.head.wait_time:
                # We waited long enough, go back
                return self.pop()
            else:
                # Represent remaining wait time
                self.repr_data["remaining_wait_time"] = connector.head.wait_time - (rospy.get_time() - self.position_reached_time)

        else:
            # We haven't reached it
            # Update when we should reach it
            self.position_reached_time = rospy.get_time()
            connector.head.send_motor_goals(angle_pan, self.pan_speed_max, angle_tilt, self.tilt_speed_max)

            # Represent remaining tilt
            self.repr_data["remaining_tilt"] = abs(current_pan_pos - angle_pan)
            self.repr_data["remaining_pan"] = abs(current_tilt_pos - angle_tilt)

        rospy.logdebug("Test")

<commit_msg>Fix wrong calculation of head tilt<commit_after># -*- coding:utf-8 -*-
"""
LookAt
^^^^^^

.. moduleauthor:: <NAME> <<EMAIL>>

This action moves the head to look at a given position on the floor relative to the robot.

The module expects a tuple containing the relative coordinates.
"""

import numpy as np
import math
import rospy
import tf2_ros as tf2
from tf2_geometry_msgs import PointStamped
from geometry_msgs.msg import Point

from bitbots_stackmachine.abstract_action_module import AbstractActionModule


class AbstractLookAt(AbstractActionModule):
    def __init__(self, connector, args):
        super(AbstractLookAt, self).__init__(connector)
        self.tfBuffer = tf2.Buffer(cache_time=rospy.Duration(10.0))
        self.listener = tf2.TransformListener(self.tfBuffer)
        self.pan_speed_max = connector.config["Head"]["Search"]["maxPanSpeedSearch"]
        self.tilt_speed_max = connector.config["Head"]["Search"]["maxTiltSpeedSearch"]
        self.position_reached_time = rospy.get_time()

    def perform(self, connector, reevaluate=False):
        raise NotImplementedError

    def look_at(self, point, connector):
        """
        This method moves the head to a given point
        :param point: a PointStamped describing the point to look at
        """
        self.repr_data = {}

        try:
            point_camera = self.tfBuffer.transform(point, 'camera_fixed')
        except (tf2.LookupException, tf2.ConnectivityException, tf2.ExtrapolationException):
            rospy.logdebug("Waiting for transform...")
            return

        # Calculate the head joint angles and clip them to the right range
        angle_pan = np.rad2deg(np.arctan2(point_camera.point.y, point_camera.point.x))
        angle_tilt = np.rad2deg(np.arctan2(point_camera.point.z,
                                           math.sqrt(point_camera.point.x ** 2 + point_camera.point.y ** 2)))
        angle_pan = np.clip(angle_pan, connector.head.min_pan, connector.head.max_pan)
        angle_tilt = np.clip(angle_tilt, connector.head.min_tilt, connector.head.max_tilt)

        current_pan_pos, current_tilt_pos = connector.head.get_current_head_pos()
        if (abs(current_pan_pos - angle_pan) < connector.head.delta and
                abs(current_tilt_pos - angle_tilt) < connector.head.delta):
            # We reached the position
            if rospy.get_time() - self.position_reached_time > connector.head.wait_time:
                # We waited long enough, go back
                return self.pop()
            else:
                # Represent remaining wait time
                self.repr_data["remaining_wait_time"] = connector.head.wait_time - (rospy.get_time() - self.position_reached_time)

        else:
            # We haven't reached it
            # Update when we should reach it
            self.position_reached_time = rospy.get_time()
            connector.head.send_motor_goals(angle_pan, self.pan_speed_max, angle_tilt, self.tilt_speed_max)

            # Represent remaining tilt
            self.repr_data["remaining_tilt"] = abs(current_pan_pos - angle_pan)
            self.repr_data["remaining_pan"] = abs(current_tilt_pos - angle_tilt)

        rospy.logdebug("Test")


{'fraction_non_alphanumeric': 0.09423750397962433, 'fraction_numerical': 0.0052531041069723014, 'mean_word_length': 2.8688423645320196, 'pattern_counts': {'":': 0, '<': 13, '<?xml version=': 0, '>': 11, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 2, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '2370677', 'n_tokens_mistral': 1913, 'n_tokens_neox': 1856, 'n_words': 417}