<commit_before>#!/usr/bin/env python

import rospy
import roslib
import rosparam
import smach
import smach_ros
import std_msgs.msg
from dynamic_reconfigure.client import Client as DynClient
from std_srvs.srv import Empty, EmptyResponse
import strands_webserver
import strands_webserver.client_utils
from mongodb_store.message_store import MessageStoreProxy
from aaf_walking_group.entertain import Entertain
from aaf_walking_group.guide_interface import GuideInterface
from aaf_walking_group.guiding import Guiding
from aaf_walking_group.reached_resting_point import RestingPoint
from aaf_walking_group.resting_chair_selection import SelectRestingChair
from aaf_walking_group.final_approach import FinalApproach
from aaf_walking_group.msg import GuidingAction, EmptyAction, StateMachineAction
from aaf_walking_group.srv import GetMediaId
from aaf_waypoint_sounds.srv import WaypointSoundsService, WaypointSoundsServiceRequest
from aaf_walking_group.utils import PTU, Gaze, RecoveryReconfigure
from aaf_walking_group.waypoint_manager import WaypointManager
import aaf_walking_group.utils as utils
from music_player.srv import MusicPlayerService
from sound_player_server.srv import PlaySoundService
from walking_group_recovery.srv import ToggleWalkingGroupRecovery
import actionlib
import json
import pprint


class WalkingGroupStateMachine(object):
    def __init__(self, name):
        rospy.loginfo("Creating " + name + " server...")
        self._as = actionlib.SimpleActionServer(
            name,
            StateMachineAction,
            self.execute,
            auto_start=False
        )
        self._as.register_preempt_callback(self.preempt_callback)

        rospy.loginfo("Creating guiding client...")
        nav_client = actionlib.SimpleActionClient("guiding", GuidingAction)
        nav_client.wait_for_server()
        rospy.loginfo(" ... done")
        rospy.loginfo("Creating wait client...")
        wait_client = actionlib.SimpleActionClient("wait_for_participant", EmptyAction)
        wait_client.wait_for_server()
        rospy.loginfo(" ... done")
        rospy.loginfo("Waiting for recovery toggle services...")
        rospy.loginfo(" ... images")
        self.rec_srv = rospy.ServiceProxy('/toggle_walking_group_recovery', ToggleWalkingGroupRecovery)
        self.rec_srv.wait_for_service()
        rospy.loginfo(" ... done")
        rospy.loginfo("Waiting for media server services...")
        rospy.loginfo(" ... images")
        s = rospy.ServiceProxy('/aaf_walking_group/image_server/get_id', GetMediaId)
        s.wait_for_service()
        rospy.loginfo(" ... video")
        s = rospy.ServiceProxy('/aaf_walking_group/video_server/get_id', GetMediaId)
        s.wait_for_service()
        rospy.loginfo(" ... sound")
        rospy.loginfo("  ... music")
        s = rospy.ServiceProxy('/music_player_service', MusicPlayerService)
        s.wait_for_service()
        rospy.loginfo("  ... jingles")
        s = rospy.ServiceProxy('/sound_player_server/sound_player_service', PlaySoundService)
        s.wait_for_service()
        rospy.loginfo("  ... waypoints")
        s = rospy.ServiceProxy('/aaf_waypoint_sounds_service', WaypointSoundsService)
        s.wait_for_service()
        rospy.loginfo(" ... done")
        # Get parameters
        self.volume = utils.get_master_volume()
        self.display_no = rospy.get_param("~display_no", 0)
        self.waypointset_name = rospy.get_param("~mongodb_params/waypointset_name", "")
        self.waypointset_collection = rospy.get_param("~mongodb_params/waypointset_collection", "aaf_walking_group")
        self.waypointset_meta = rospy.get_param("~mongodb_params/waypointset_meta", "waypoint_set")
        if self.waypointset_name == "":
            rospy.logfatal("Missing parameters.")
            rospy.logfatal("Please run with _mongodb_params/waypointset_name:=<waypointset_name>")
            return

        self.preempt_srv = None

        self.ptu = PTU()
        self.gaze = Gaze()
        self.recovery = RecoveryReconfigure(
            name="/monitored_navigation/recover_states/",
            whitelist=rosparam.load_file(rospy.get_param("~recovery_whitelist"))[0][0]["recover_states"]
        )
        rospy.loginfo("Creating dynamic reconfigure clients")
        self.han_dyn_client = DynClient(
            "/human_aware_navigation"
        )
        self.mov_dyn_client = DynClient(
            "/human_aware_navigation/DWAPlannerROS"
        )
        rospy.loginfo(" ...done")
        self.get_current_dyn_settings()

        rospy.loginfo(" ... starting " + name)
        self._as.start()
        rospy.loginfo(" ... started " + name)

    def get_current_dyn_settings(self):
        gazing = rospy.get_param("/human_aware_navigation/gaze_type")
        angle = round(rospy.get_param("/human_aware_navigation/detection_angle"),2)
        self.han_param = {
            'gaze_type': gazing,
            'detection_angle': angle
        }
        rospy.loginfo("Found following default values for human_aware_navigation: %s", self.han_param)
        max_vel_x = round(rospy.get_param("/human_aware_navigation/DWAPlannerROS/max_vel_x"), 2)
        max_trans_vel = round(rospy.get_param("/human_aware_navigation/DWAPlannerROS/max_trans_vel"),2)
        self.mov_param = {
            "max_vel_x": max_vel_x,
            "max_trans_vel": max_trans_vel
        }
        rospy.loginfo("Found following default values for human_aware_navigation/DWAPlannerROS: %s", self.mov_param)

    def execute(self, goal):
        rospy.loginfo("Starting state machine")

        try:
            rospy.loginfo("Creating recovery toggle service proxy and waiting ...")
            self.rec_srv.wait_for_service()
            rospy.loginfo(" .. calling recovery toggle service")
            self.rec_srv(True)
            rospy.loginfo(" .. started walking group recovery")
        except (rospy.ServiceException, rospy.ROSInterruptException) as e:
            rospy.logwarn("waypoint sound service call failed: %s" % e)
        #Save current recovery behaviour states
        self.recovery.save_current_configuration()
        #and reconfigure
        self.recovery.reconfigure(RecoveryReconfigure.SET)

        # Setting http root
        http_root = roslib.packages.get_pkg_dir('aaf_walking_group') + '/www'
        strands_webserver.client_utils.set_http_root(http_root)

        self.ptu.turnPTU(-180, 10)
        dyn_param = {
            'timeout': 0.0,
            'gaze_type': 1,
            'detection_angle': 20.0
        }
        try:<commit_msg>Lowering camera angle<commit_after>#!/usr/bin/env python

import rospy
import roslib
import rosparam
import smach
import smach_ros
import std_msgs.msg
from dynamic_reconfigure.client import Client as DynClient
from std_srvs.srv import Empty, EmptyResponse
import strands_webserver
import strands_webserver.client_utils
from mongodb_store.message_store import MessageStoreProxy
from aaf_walking_group.entertain import Entertain
from aaf_walking_group.guide_interface import GuideInterface
from aaf_walking_group.guiding import Guiding
from aaf_walking_group.reached_resting_point import RestingPoint
from aaf_walking_group.resting_chair_selection import SelectRestingChair
from aaf_walking_group.final_approach import FinalApproach
from aaf_walking_group.msg import GuidingAction, EmptyAction, StateMachineAction
from aaf_walking_group.srv import GetMediaId
from aaf_waypoint_sounds.srv import WaypointSoundsService, WaypointSoundsServiceRequest
from aaf_walking_group.utils import PTU, Gaze, RecoveryReconfigure
from aaf_walking_group.waypoint_manager import WaypointManager
import aaf_walking_group.utils as utils
from music_player.srv import MusicPlayerService
from sound_player_server.srv import PlaySoundService
from walking_group_recovery.srv import ToggleWalkingGroupRecovery
import actionlib
import json
import pprint


class WalkingGroupStateMachine(object):
    def __init__(self, name):
        rospy.loginfo("Creating " + name + " server...")
        self._as = actionlib.SimpleActionServer(
            name,
            StateMachineAction,
            self.execute,
            auto_start=False
        )
        self._as.register_preempt_callback(self.preempt_callback)

        rospy.loginfo("Creating guiding client...")
        nav_client = actionlib.SimpleActionClient("guiding", GuidingAction)
        nav_client.wait_for_server()
        rospy.loginfo(" ... done")
        rospy.loginfo("Creating wait client...")
        wait_client = actionlib.SimpleActionClient("wait_for_participant", EmptyAction)
        wait_client.wait_for_server()
        rospy.loginfo(" ... done")
        rospy.loginfo("Waiting for recovery toggle services...")
        rospy.loginfo(" ... images")
        self.rec_srv = rospy.ServiceProxy('/toggle_walking_group_recovery', ToggleWalkingGroupRecovery)
        self.rec_srv.wait_for_service()
        rospy.loginfo(" ... done")
        rospy.loginfo("Waiting for media server services...")
        rospy.loginfo(" ... images")
        s = rospy.ServiceProxy('/aaf_walking_group/image_server/get_id', GetMediaId)
        s.wait_for_service()
        rospy.loginfo(" ... video")
        s = rospy.ServiceProxy('/aaf_walking_group/video_server/get_id', GetMediaId)
        s.wait_for_service()
        rospy.loginfo(" ... sound")
        rospy.loginfo("  ... music")
        s = rospy.ServiceProxy('/music_player_service', MusicPlayerService)
        s.wait_for_service()
        rospy.loginfo("  ... jingles")
        s = rospy.ServiceProxy('/sound_player_server/sound_player_service', PlaySoundService)
        s.wait_for_service()
        rospy.loginfo("  ... waypoints")
        s = rospy.ServiceProxy('/aaf_waypoint_sounds_service', WaypointSoundsService)
        s.wait_for_service()
        rospy.loginfo(" ... done")
        # Get parameters
        self.volume = utils.get_master_volume()
        self.display_no = rospy.get_param("~display_no", 0)
        self.waypointset_name = rospy.get_param("~mongodb_params/waypointset_name", "")
        self.waypointset_collection = rospy.get_param("~mongodb_params/waypointset_collection", "aaf_walking_group")
        self.waypointset_meta = rospy.get_param("~mongodb_params/waypointset_meta", "waypoint_set")
        if self.waypointset_name == "":
            rospy.logfatal("Missing parameters.")
            rospy.logfatal("Please run with _mongodb_params/waypointset_name:=<waypointset_name>")
            return

        self.preempt_srv = None

        self.ptu = PTU()
        self.gaze = Gaze()
        self.recovery = RecoveryReconfigure(
            name="/monitored_navigation/recover_states/",
            whitelist=rosparam.load_file(rospy.get_param("~recovery_whitelist"))[0][0]["recover_states"]
        )
        rospy.loginfo("Creating dynamic reconfigure clients")
        self.han_dyn_client = DynClient(
            "/human_aware_navigation"
        )
        self.mov_dyn_client = DynClient(
            "/human_aware_navigation/DWAPlannerROS"
        )
        rospy.loginfo(" ...done")
        self.get_current_dyn_settings()

        rospy.loginfo(" ... starting " + name)
        self._as.start()
        rospy.loginfo(" ... started " + name)

    def get_current_dyn_settings(self):
        gazing = rospy.get_param("/human_aware_navigation/gaze_type")
        angle = round(rospy.get_param("/human_aware_navigation/detection_angle"),2)
        self.han_param = {
            'gaze_type': gazing,
            'detection_angle': angle
        }
        rospy.loginfo("Found following default values for human_aware_navigation: %s", self.han_param)
        max_vel_x = round(rospy.get_param("/human_aware_navigation/DWAPlannerROS/max_vel_x"), 2)
        max_trans_vel = round(rospy.get_param("/human_aware_navigation/DWAPlannerROS/max_trans_vel"),2)
        self.mov_param = {
            "max_vel_x": max_vel_x,
            "max_trans_vel": max_trans_vel
        }
        rospy.loginfo("Found following default values for human_aware_navigation/DWAPlannerROS: %s", self.mov_param)

    def execute(self, goal):
        rospy.loginfo("Starting state machine")

        try:
            rospy.loginfo("Creating recovery toggle service proxy and waiting ...")
            self.rec_srv.wait_for_service()
            rospy.loginfo(" .. calling recovery toggle service")
            self.rec_srv(True)
            rospy.loginfo(" .. started walking group recovery")
        except (rospy.ServiceException, rospy.ROSInterruptException) as e:
            rospy.logwarn("waypoint sound service call failed: %s" % e)
        #Save current recovery behaviour states
        self.recovery.save_current_configuration()
        #and reconfigure
        self.recovery.reconfigure(RecoveryReconfigure.SET)

        # Setting http root
        http_root = roslib.packages.get_pkg_dir('aaf_walking_group') + '/www'
        strands_webserver.client_utils.set_http_root(http_root)

        self.ptu.turnPTU(-180, 15)
        dyn_param = {
            'timeout': 0.0,
            'gaze_type': 1,
            'detection_angle': 20.0
        }
        try:
{'fraction_non_alphanumeric': 0.09702517162471395, 'fraction_numerical': 0.002593440122044241, 'mean_word_length': 3.098468271334792, 'pattern_counts': {'":': 6, '<': 5, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 2, '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': '1460836', 'n_tokens_mistral': 4152, 'n_tokens_neox': 4023, 'n_words': 689}