<commit_before>#!/usr/bin/env python
# Using Black: https://github.com/ambv/black
# pylint: disable=line-too-long
# Software License Agreement (BSD License)
#
# Author: <NAME> https://github.com/chrisl8
# URL: https://github.com/chrisl8/ArloBot
#
# Derived from \opt\ros\hydro\lib\create_node\turtlebot_node.py
# This is based on turtlebot_node adapted to run on a Propeller Activity Board based ArloBot
#
# Special thanks to arduino.py by Dr. <NAME>
# https://code.google.com/p/drh-robotics-ros/
#
# NOTE: This script requires parameters to be loaded from param/arlobot.yaml!

from __future__ import print_function
import rospy
import tf
from math import sin, cos
import time

from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from arlobot_msgs.msg import usbRelayStatus, arloStatus, arloSafety, arloButtons
from arlobot_msgs.srv import FindRelay, ToggleRelay, ToggleLED

from PropellerSerialInterface import PropellerSerialInterface
from PropellerSerialDataPacketTypes import PropellerSerialDataPacketTypes
from OdomStationaryBroadcaster import OdomStationaryBroadcaster


class PropellerComm(object):
    """
    Helper class for communicating with a Propeller board over serial port
    """

    def __init__(self):
        rospy.init_node("arlobot")

        self.r = rospy.Rate(1)  # 1hz refresh rate
        self._motorsOn = (
            False
        )  # Set to True if the motors are on, used with USB Relay Control board
        self._safeToGo = False  # Use arlobot_safety to set this
        self._SafeToOperate = False  # Use arlobot_safety to set this
        self._unPlugging = False  # Used for when arlobot_safety tells us to "UnPlug"!
        self._wasUnplugging = (
            False
        )  # Track previous unplugging status for motor control
        self._SwitchingMotors = False  # Prevent overlapping calls to _switch_motors
        self._serialAvailable = False
        self._odometry_broadcast_timeout = 0
        self._odometry_broadcast_timeout_max = 2
        self._broadcast_static_odometry = False
        self._leftMotorPower = False
        self._rightMotorPower = False
        self._laptop_battery_percent = 100
        # Store last x, y and heading for reuse when we reset
        # I took off the ~, because that was causing these to reset to default on every restart
        # even if roscore was still up.
        self.lastX = rospy.get_param("lastX", 0.0)
        self.lastY = rospy.get_param("lastY", 0.0)
        self.lastHeading = rospy.get_param("lastHeading", 0.0)
        self.alternate_heading = self.lastHeading

        # Get motor relay numbers for use later in _HandleUSBRelayStatus if USB Relay is in use:
        self.relayExists = rospy.get_param("~usbRelayInstalled", False)
        if self.relayExists:
            # I think it is better to get these once than on every run of _HandleUSBRelayStatus
            self.usbLeftMotorRelayLabel = rospy.get_param("~usbLeftMotorRelayLabel", "")
            self.usbRightMotorRelayLabel = rospy.get_param(
                "~usbRightMotorRelayLabel", ""
            )
            rospy.loginfo("Waiting for USB Relay find_relay service to start . . .")
            rospy.wait_for_service("/arlobot_usbrelay/find_relay")
            rospy.loginfo("USB Relay find_relay service started.")
            try:
                find_relay = rospy.ServiceProxy(
                    "/arlobot_usbrelay/find_relay", FindRelay
                )
                self.leftMotorRelay = find_relay(self.usbLeftMotorRelayLabel)
                self.rightMotorRelay = find_relay(self.usbRightMotorRelayLabel)
                if self.leftMotorRelay.foundRelay and self.leftMotorRelay.foundRelay:
                    rospy.loginfo(
                        "Left = "
                        + str(self.leftMotorRelay.relayNumber)
                        + " & Right = "
                        + str(self.rightMotorRelay.relayNumber)
                    )
                else:
                    self.relayExists = False
            except rospy.ServiceException as e:
                rospy.loginfo("Service call failed: %s" % e)
            rospy.Subscriber(
                "arlobot_usbrelay/usbRelayStatus",
                usbRelayStatus,
                self._handle_usb_relay_status,
            )  # Safety Shutdown

        # Store the data from ROS
        self._settings_from_ros = {
            "trackWidth": 0.0,
            "distancePerCount": 0.0,
            "ignoreProximity": False,
            "ignoreCliffSensors": False,
            "ignoreIRSensors": False,
            "ignoreFloorSensors": False,
            "pluggedIn": True,
        }

        self._updateSettingsFromROS()

        # Store the incoming data from the Propeller board
        self._config_from_propeller = {
            "trackWidth": 0,
            "distancePerCount": 0,
            # Set to opposites of self._settings
            # to ensure _settingsUpdateRequired stays true
            # until we get the data from the board.
            "ignoreProximity": not self._settings_from_ros["ignoreProximity"],
            "ignoreCliffSensors": not self._settings_from_ros["ignoreCliffSensors"],
            "ignoreIRSensors": not self._settings_from_ros["ignoreIRSensors"],
            "ignoreFloorSensors": not self._settings_from_ros["ignoreFloorSensors"],
            "pluggedIn": not self._settings_from_ros["pluggedIn"],
        }

        self._ledInputData_from_propeller = []
        self._ledRequestedState_from_ROS = []

        # Subscriptions
        rospy.Subscriber("cmd_vel", Twist, self._handle_velocity_command, queue_size=1)
        # NOTE: Keep the cmd_vel subscription queue_size to 1, because we never want to allow a backlog of twist commands. Always just send the most recent one.
        # Otherwise strange behavior occurs, even the robot responding to commands very late, like stopping and then starting a command given moments ago!
        rospy.Subscriber(
            "arlobot_safety/safetyStatus",
            arloSafety,
            self._safety_shutdown,
            queue_size=1,
        )  # Safety Shutdown

        # Publishers
        # for publishing PIR status
        # self._pirPublisher = rospy.Publisher("~pirState", Bool, queue_size=1)
        self._arlo_status_publisher = rospy.Publisher(
            "arlo_status", arloStatus, queue_size=1
        )
        self._buttons_publisher = rospy.Publisher("buttons", arloButtons, queue_size=1)

        # IF the Odometry Transform is done with the robot_pose_ekf do not publish it,
        # but we are not using robot_pose_ekf, because it does nothing for us if you don't have a full IMU!
        # REMOVE this line if you use robot_pose_ekf
        self._OdometryTransformBroadcaster = tf.TransformBroadcaster()

        self._OdometryPublisher = rospy.Publisher("odom", Odometry, queue_size=1)

        # We don't need to broadcast a transform, as it is static and contained within the URDF files
        # self._SonarTransformBroadcaster = tf.TransformBroadcaster()
        self._UltraSonicPublisher = rospy.Publisher(
            "ultrasonic_scan", LaserScan, queue_size=1
        )
        self._InfraredPublisher = rospy.Publisher(
            "infrared_scan", LaserScan, queue_size=1
        )

        # For Camera Joint Testing
        # self._JointStatePublisher = rospy.Publisher(<commit_msg>Change to TF2.<commit_after>#!/usr/bin/env python
# Using Black: https://github.com/ambv/black
# pylint: disable=line-too-long
# Software License Agreement (BSD License)
#
# Author: <NAME> https://github.com/chrisl8
# URL: https://github.com/chrisl8/ArloBot
#
# Derived from \opt\ros\hydro\lib\create_node\turtlebot_node.py
# This is based on turtlebot_node adapted to run on a Propeller Activity Board based ArloBot
#
# Special thanks to arduino.py by Dr. <NAME>
# https://code.google.com/p/drh-robotics-ros/
#
# NOTE: This script requires parameters to be loaded from param/arlobot.yaml!

from __future__ import print_function
import rospy
import tf2_ros
from math import sin, cos
import time

from geometry_msgs.msg import Quaternion
from geometry_msgs.msg import Twist
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry
from arlobot_msgs.msg import usbRelayStatus, arloStatus, arloSafety, arloButtons
from arlobot_msgs.srv import FindRelay, ToggleRelay, ToggleLED

from PropellerSerialInterface import PropellerSerialInterface
from PropellerSerialDataPacketTypes import PropellerSerialDataPacketTypes
from OdomStationaryBroadcaster import OdomStationaryBroadcaster


class PropellerComm(object):
    """
    Helper class for communicating with a Propeller board over serial port
    """

    def __init__(self):
        rospy.init_node("arlobot")

        self.r = rospy.Rate(1)  # 1hz refresh rate
        self._motorsOn = (
            False
        )  # Set to True if the motors are on, used with USB Relay Control board
        self._safeToGo = False  # Use arlobot_safety to set this
        self._SafeToOperate = False  # Use arlobot_safety to set this
        self._unPlugging = False  # Used for when arlobot_safety tells us to "UnPlug"!
        self._wasUnplugging = (
            False
        )  # Track previous unplugging status for motor control
        self._SwitchingMotors = False  # Prevent overlapping calls to _switch_motors
        self._serialAvailable = False
        self._odometry_broadcast_timeout = 0
        self._odometry_broadcast_timeout_max = 2
        self._broadcast_static_odometry = False
        self._leftMotorPower = False
        self._rightMotorPower = False
        self._laptop_battery_percent = 100
        # Store last x, y and heading for reuse when we reset
        # I took off the ~, because that was causing these to reset to default on every restart
        # even if roscore was still up.
        self.lastX = rospy.get_param("lastX", 0.0)
        self.lastY = rospy.get_param("lastY", 0.0)
        self.lastHeading = rospy.get_param("lastHeading", 0.0)
        self.alternate_heading = self.lastHeading

        # Get motor relay numbers for use later in _HandleUSBRelayStatus if USB Relay is in use:
        self.relayExists = rospy.get_param("~usbRelayInstalled", False)
        if self.relayExists:
            # I think it is better to get these once than on every run of _HandleUSBRelayStatus
            self.usbLeftMotorRelayLabel = rospy.get_param("~usbLeftMotorRelayLabel", "")
            self.usbRightMotorRelayLabel = rospy.get_param(
                "~usbRightMotorRelayLabel", ""
            )
            rospy.loginfo("Waiting for USB Relay find_relay service to start . . .")
            rospy.wait_for_service("/arlobot_usbrelay/find_relay")
            rospy.loginfo("USB Relay find_relay service started.")
            try:
                find_relay = rospy.ServiceProxy(
                    "/arlobot_usbrelay/find_relay", FindRelay
                )
                self.leftMotorRelay = find_relay(self.usbLeftMotorRelayLabel)
                self.rightMotorRelay = find_relay(self.usbRightMotorRelayLabel)
                if self.leftMotorRelay.foundRelay and self.leftMotorRelay.foundRelay:
                    rospy.loginfo(
                        "Left = "
                        + str(self.leftMotorRelay.relayNumber)
                        + " & Right = "
                        + str(self.rightMotorRelay.relayNumber)
                    )
                else:
                    self.relayExists = False
            except rospy.ServiceException as e:
                rospy.loginfo("Service call failed: %s" % e)
            rospy.Subscriber(
                "arlobot_usbrelay/usbRelayStatus",
                usbRelayStatus,
                self._handle_usb_relay_status,
            )  # Safety Shutdown

        # Store the data from ROS
        self._settings_from_ros = {
            "trackWidth": 0.0,
            "distancePerCount": 0.0,
            "ignoreProximity": False,
            "ignoreCliffSensors": False,
            "ignoreIRSensors": False,
            "ignoreFloorSensors": False,
            "pluggedIn": True,
        }

        self._updateSettingsFromROS()

        # Store the incoming data from the Propeller board
        self._config_from_propeller = {
            "trackWidth": 0,
            "distancePerCount": 0,
            # Set to opposites of self._settings
            # to ensure _settingsUpdateRequired stays true
            # until we get the data from the board.
            "ignoreProximity": not self._settings_from_ros["ignoreProximity"],
            "ignoreCliffSensors": not self._settings_from_ros["ignoreCliffSensors"],
            "ignoreIRSensors": not self._settings_from_ros["ignoreIRSensors"],
            "ignoreFloorSensors": not self._settings_from_ros["ignoreFloorSensors"],
            "pluggedIn": not self._settings_from_ros["pluggedIn"],
        }

        self._ledInputData_from_propeller = []
        self._ledRequestedState_from_ROS = []

        # Subscriptions
        rospy.Subscriber("cmd_vel", Twist, self._handle_velocity_command, queue_size=1)
        # NOTE: Keep the cmd_vel subscription queue_size to 1, because we never want to allow a backlog of twist commands. Always just send the most recent one.
        # Otherwise strange behavior occurs, even the robot responding to commands very late, like stopping and then starting a command given moments ago!
        rospy.Subscriber(
            "arlobot_safety/safetyStatus",
            arloSafety,
            self._safety_shutdown,
            queue_size=1,
        )  # Safety Shutdown

        # Publishers
        # for publishing PIR status
        # self._pirPublisher = rospy.Publisher("~pirState", Bool, queue_size=1)
        self._arlo_status_publisher = rospy.Publisher(
            "arlo_status", arloStatus, queue_size=1
        )
        self._buttons_publisher = rospy.Publisher("buttons", arloButtons, queue_size=1)

        # IF the Odometry Transform is done with the robot_pose_ekf do not publish it,
        # but we are not using robot_pose_ekf, because it does nothing for us if you don't have a full IMU!
        # REMOVE this line if you use robot_pose_ekf
        self._OdometryTransformBroadcaster = tf2_ros.TransformBroadcaster()

        self._OdometryPublisher = rospy.Publisher("odom", Odometry, queue_size=1)

        # We don't need to broadcast a transform, as it is static and contained within the URDF files
        # self._SonarTransformBroadcaster = tf2_ros.TransformBroadcaster()
        self._UltraSonicPublisher = rospy.Publisher(
            "ultrasonic_scan", LaserScan, queue_size=1
        )
        self._InfraredPublisher = rospy.Publisher(
            "infrared_scan", LaserScan, queue_size=1
        )

        # For Camera Joint Testing
        # self._JointStatePublisher = rospy.Publisher(
{'fraction_non_alphanumeric': 0.0732147633056967, 'fraction_numerical': 0.004279219042524739, 'mean_word_length': 2.463872163038444, 'pattern_counts': {'":': 28, '<': 7, '<?xml version=': 0, '>': 7, 'https://': 8, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '1689688', 'n_tokens_mistral': 4549, 'n_tokens_neox': 4261, 'n_words': 1123}