diff --git a/demos/pycram_receptionist_demo/receptionist_clean_code.py b/demos/pycram_receptionist_demo/receptionist_clean_code.py new file mode 100644 index 000000000..e1e8953cb --- /dev/null +++ b/demos/pycram_receptionist_demo/receptionist_clean_code.py @@ -0,0 +1,176 @@ +from std_msgs.msg import String + +from demos.pycram_receptionist_demo.utils.NLP_Functions import NLP_Functions +from demos.pycram_receptionist_demo.utils.helper import * +from pycram.designators.action_designator import * +from pycram.designators.motion_designator import * +from pycram.designators.object_designator import * +from pycram.process_module import real_robot +from pycram.ros_utils.object_state_updater import RobotStateUpdater +from pycram.ros_utils.viz_marker_publisher import VizMarkerPublisher +from pycram.utilities.robocup_utils import ImageSwitchPublisher +from pycram.world_concepts.world_object import Object +from pycram.worlds.bullet_world import BulletWorld +import rospy +from pycrap import Robot + +# Initialize the Bullet world for simulation +world = BulletWorld() + +# Visualization Marker Publisher for ROS +v = VizMarkerPublisher() + +# Create and configure the robot object +robot = Object("hsrb", Robot, "../../resources/hsrb.urdf", pose=Pose([0, 0, 0])) +RobotStateUpdater("/tf", "/giskard_joint_states") +image_switch_publisher = ImageSwitchPublisher() + +# Create environmental objects +apartment = Object("kitchen", ObjectType.ENVIRONMENT, "suturo_lab_2.urdf") + +# class object for communication with nlp +nlp = NLP_Functions() + +# Declare variables for humans +host = HumanDescription("Bob", fav_drink="Milk") +host.set_id(1) + +guest1 = HumanDescription("Lisa", fav_drink="water") +guest1.set_attributes(['male', 'without a hat', 'wearing a t-shirt', ' a dark top']) +guest1.set_id(0) + +guest2 = HumanDescription("Sarah", fav_drink="Juice") +guest2.set_attributes(['female', 'with a hat', 'wearing a t-shirt', ' a bright top']) + +# important poses +couch_pose_semantik = Pose(position=[3.8, 2.1, 0], orientation=[0, 0, -0.7, 0.7]) +look_couch = Pose([3.8, 0.3, 0.75]) +nav_pose1 = Pose([2, 1.3, 0], orientation=[0, 0, 0.4, 0.9]) +greet_guest_pose = Pose(position=[1.9, 0.6, 0], orientation=[0, 0, 0.8, -0.5]) + + +def demo(step: int): + with (real_robot): + rospy.loginfo("start demo at step " + str(step)) + + # set neutral pose + image_switch_publisher.pub_now(ImageEnum.HI.value) + MoveJointsMotion(["head_tilt_joint"], [0.0]).perform() + ParkArmsAction([Arms.LEFT]).resolve().perform() + MoveJointsMotion(["torso_lift_joint"], [0.0]).perform() + + if step <= 1: + # greet first guest + nlp.welcome_guest(guest1) + + if step <= 2: + # perceive attributes of guest + MoveJointsMotion(["torso_lift_joint"], [0.0]).perform() + get_attributes(guest1) + + if step <= 3: + # lead to living room + TalkingMotion("i will show you the living room now").perform() + rospy.sleep(1.5) + TalkingMotion("please step out of the way and follow me").perform() + NavigateAction([couch_pose_semantik]).resolve().perform() + + if step <= 4: + # find host in living room + TalkingMotion("welcome to the living room").perform() + + # try to find face (of host) in living room + counter = 0 + while counter < 6: + detected = detect_host_face(host) + counter += 1 + if detected: + break + if counter == 1: + TalkingMotion("sitting people please look at me").perform() + rospy.sleep(1.5) + + elif counter == 2: + # look to the side to find face + MoveJointsMotion(["head_pan_joint"], [-0.3]).perform() + TalkingMotion("please look at me").perform() + rospy.sleep(1.5) + + if counter == 5: + try: + rospy.logerr("host has no id") + host_pose = DetectAction(technique='human').resolve().perform() + host.set_pose(host_pose) + + except Exception as e: + print(e) + break + + counter += 1 + + if step <= 5: + # find free place to sit for guest + LookAtAction([look_couch]).resolve().perform() + guest_pose = detect_point_to_seat(robot) + if not guest_pose: + # look to the side to find seat + MoveJointsMotion(["head_pan_joint"], [-0.3]).perform() + guest_pose = detect_point_to_seat(no_sofa=True, robot=robot) + guest1.set_pose(guest_pose) + else: + guest1.set_pose(guest_pose) + + if step <= 6: + # introduce sitting people + HeadFollowMotion(state="start").perform() + rospy.sleep(2) + introduce(host, guest1) + rospy.sleep(2) + HeadFollowMotion(state="stop").perform() + MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() + + if step <= 7: + # go back to start-pose + ParkArmsAction([Arms.LEFT]).resolve().perform() + NavigateAction([greet_guest_pose]).resolve().perform() + TalkingMotion("waiting for new guest").perform() + image_switch_publisher.pub_now(ImageEnum.HI.value) + + if step <= 8: + # greet second guest and lead to living room + nlp.welcome_guest(guest2) + MoveJointsMotion(["torso_lift_joint"], [0.0]).perform() + TalkingMotion("i will show you the living room now").perform() + rospy.sleep(1.5) + TalkingMotion("please step out of the way and follow me").perform() + NavigateAction([couch_pose_semantik]).resolve().perform() + + if step <= 9: + # search for host and guest + TalkingMotion("welcome to the living room").perform() + identify_faces(host, guest1) + + if step <= 10: + # find free place for second guest + LookAtAction([look_couch]).resolve().perform() + guest_pose = detect_point_to_seat(robot) + if not guest_pose: + MoveJointsMotion(["head_pan_joint"], [-0.3]).perform() + guest_pose = detect_point_to_seat(no_sofa=True) + guest2.set_pose(guest_pose) + else: + guest2.set_pose(guest_pose) + + if step <= 11: + # introduce everyone and state attributes of first guest + HeadFollowMotion(state="start").perform() + rospy.sleep(1.5) + introduce(host, guest2) + rospy.sleep(3) + introduce(guest1, guest2) + rospy.sleep(3) + describe(guest1) + MoveGripperMotion(GripperState.OPEN, Arms.LEFT).perform() + + +demo(0) diff --git a/demos/pycram_receptionist_demo/utils/NLP_Functions.py b/demos/pycram_receptionist_demo/utils/NLP_Functions.py new file mode 100644 index 000000000..57e683e34 --- /dev/null +++ b/demos/pycram_receptionist_demo/utils/NLP_Functions.py @@ -0,0 +1,224 @@ +import time +from std_msgs.msg import String +from pycram.designators.action_designator import * +from pycram.designators.motion_designator import * +from pycram.designators.object_designator import HumanDescription +from pycram.utilities.robocup_utils import ImageSwitchPublisher + +response = [None, None, None] +callback = False +timeout = 10 + + +class NLP_Functions: + """ + Class that stores important nlp functions for receptionist + """ + + def __init__(self): + self.nlp_pub = rospy.Publisher('/startListener', String, queue_size=16) + self.sub_nlp = rospy.Subscriber("nlp_out", String, self.data_cb) + self.response = ["", ""] + self.callback = False + self.image_switch_publisher = ImageSwitchPublisher() + + def data_cb(self, data): + """ + function to receive data from nlp via /nlp_out topic + """ + self.image_switch_publisher.pub_now(ImageEnum.HI.value) + self.response = data.data.split(",") + for ele in self.response: + ele.strip() + self.response.append("None") + rospy.loginfo(self.response) + self.callback = True + + def welcome_guest(self, guest: HumanDescription): + """ + talking sequence to get name and favorite drink of guest + :param guest: variable to store new information about human + """ + + TalkingMotion("Welcome, please step in front of me and come close").perform() + + # look for human and position higher + DetectAction(technique='human').resolve().perform() + rospy.sleep(1) + MoveJointsMotion(["torso_lift_joint"], [0.2]).perform() + + # look at guest and introduce + HeadFollowMotion(state="start").perform() + rospy.sleep(2.3) + HeadFollowMotion(state="start").perform() + + TalkingMotion("Hello, i am Toya and my favorite drink is oil.").perform() + rospy.sleep(2.5) + TalkingMotion("What is your name and favorite drink?").perform() + rospy.sleep(2.5) + TalkingMotion("please answer me when my display changes").perform() + rospy.sleep(2.5) + + # signal to start listening + rospy.loginfo("nlp start") + self.nlp_pub.publish("start listening") + rospy.sleep(2.2) + self.image_switch_publisher.pub_now(ImageEnum.TALK.value) + + # wait for nlp answer + start_time = time.time() + while not self.callback: + rospy.sleep(1) + + if int(time.time() - start_time) == timeout: + rospy.logwarn("guest needs to repeat") + self.image_switch_publisher.pub_now(ImageEnum.JREPEAT.value) + + self.callback = False + + # check response -> was everything understood with right intent + if self.response[0] == "": + # success a name and intent was understood + if self.response[1].strip() != "None" and self.response[2].strip() != "None": + # understood both + guest.set_drink(self.response[2]) + guest.set_name(self.response[1]) + else: + name = False + drink = False + if self.response[1].strip() == "None": + # ask for name again once + name = True + guest.set_drink(self.response[2]) + + if self.response[2].strip() == "None": + # ask for drink again + drink = True + guest.set_name(self.response[1]) + + if name: + guest.set_name(self.name_repeat()) + + if drink: + guest.set_drink(self.drink_repeat()) + + else: + # two chances to get name and drink + i = 0 + while i < 2: + TalkingMotion("please repeat your name and drink loud and clear").perform() + rospy.sleep(2.1) + + self.nlp_pub.publish("start") + rospy.sleep(2.5) + self.image_switch_publisher.pub_now(ImageEnum.TALK.value) + + start_time = time.time() + while not self.callback: + rospy.sleep(1) + if int(time.time() - start_time) == timeout: + rospy.logwarn("guest needs to repeat") + self.image_switch_publisher.pub_now(ImageEnum.JREPEAT.value) + self.callback = False + + if self.response[0] == "": + # success a name and intent was understood + if self.response[1].strip() != "None" and self.response[2].strip() != "None": + # understood both + guest.set_drink(self.response[2]) + guest.set_name(self.response[1]) + break + else: + name = False + drink = False + if self.response[1].strip() == "None": + # ask for name again once + name = True + guest.set_drink(self.response[2]) + + if self.response[2].strip() == "None": + drink = True + # ask for drink again + guest.set_name(self.response[1]) + + if name: + guest.set_name(self.name_repeat()) + break + + if drink: + guest.set_drink(self.drink_repeat()) + break + + HeadFollowMotion(state="stop").perform() + DetectAction(technique='human', state="stop").resolve().perform() + rospy.sleep(1) + TalkingMotion("Nice to meet you").perform() + return guest + + def name_repeat(self): + """ + HRI-function to ask for name again once. + """ + + self.callback = False + trys = 0 + + while trys < 2: + TalkingMotion("i am sorry, please repeat your name").perform() + rospy.sleep(1.2) + + self.nlp_pub.publish("start") + rospy.sleep(2.5) + self.image_switch_publisher.pub_now(ImageEnum.TALK.value) + + # wait for response + start_time = time.time() + while not self.callback: + # signal repeat to human + if time.time() - start_time == timeout: + rospy.logwarn("guest needs to repeat") + self.image_switch_publisher.pub_now(ImageEnum.JREPEAT.value) + + self.image_switch_publisher.pub_now(ImageEnum.HI.value) + self.callback = False + + if self.response[0] == "" and self.response[1].strip() != "None": + return self.response[1] + + trys += 1 + + def drink_repeat(self): + """ + HRI-function to ask for drink again. + """ + + self.callback = False + trys = 0 + + while trys < 2: + TalkingMotion("i am sorry, please repeat your drink loud and clear").perform() + rospy.sleep(3.5) + TalkingMotion("please use the sentence my favorite drink is").perform() + rospy.sleep(3) + + self.nlp_pub.publish("start") + rospy.sleep(2.5) + self.image_switch_publisher.pub_now(ImageEnum.TALK.value) + + # wait for response + start_time = time.time() + while not self.callback: + if time.time() - start_time == timeout: + rospy.logwarn("guest needs to repeat") + self.image_switch_publisher.pub_now(ImageEnum.JREPEAT.value) + + self.image_switch_publisher.pub_now(ImageEnum.HI.value) + self.callback = False + + if self.response[0] == "" and self.response[2].strip() != "None": + trys += 1 + return self.response[2] + + trys += 1 + + return "water" diff --git a/demos/pycram_receptionist_demo/utils/helper.py b/demos/pycram_receptionist_demo/utils/helper.py new file mode 100644 index 000000000..219d7b5f5 --- /dev/null +++ b/demos/pycram_receptionist_demo/utils/helper.py @@ -0,0 +1,309 @@ +from typing import Optional +from geometry_msgs.msg import PointStamped, PoseStamped +from pycram.designators.action_designator import * +from pycram.designators.motion_designator import PointingMotion +from pycram.designators.object_designator import HumanDescription +from pycram.failures import PerceptionObjectNotFound + +look_couch = Pose([3.8, 0.3, 0.75]) + + +def get_attributes(guest: HumanDescription, trys: Optional[int] = 0): + """ + storing attributes and face of person in front of robot + :param guest: variable to store information in + :param trys: keep track of failure handling + """ + MoveJointsMotion(["head_pan_joint"], [0.0]).perform() + MoveJointsMotion(["head_tilt_joint"], [0.45]).perform() + TalkingMotion("i will take a picture of you to recognize you later").perform() + rospy.sleep(2.4) + TalkingMotion("please look at me").perform() + rospy.sleep(1.5) + # remember face + while trys < 1: + try: + # get an ID for face + keys = DetectAction(technique='human', state='face').resolve().perform() + new_id = keys["keys"][0] + guest.set_id(new_id) + + # get 4 different attributes + attr_list = DetectAction(technique='attributes', state='start').resolve().perform() + rospy.loginfo(attr_list) + guest.set_attributes(attr_list) + rospy.loginfo(attr_list) + break + + except PerceptionObjectNotFound: + # failure handling, if human has stepped away + TalkingMotion("please step in front of me").perform() + rospy.sleep(3.5) + trys += 1 + + try: + get_attributes(guest, trys=trys) + + except PerceptionObjectNotFound: + trys += 1 + rospy.logerr("continue without attributes") + + return guest + + +def detect_point_to_seat(robot, no_sofa: Optional[bool] = False): + """ + function to look for a place to sit and poit to it + returns bool if free place found or not + :param robot: robot-object used in the demo + :param no_sofa: if true, free seats on sofa get ignored + """ + + # detect free seat + seat = DetectAction(technique='location', state="sofa").resolve().perform() + free_seat = False + + # loop through all seating options detected by perception + if not no_sofa: + for place in seat: + # found a place that is not occupied + if place[1] == 'False': + + pose_in_map = Pose([float(place[2]), float(place[3]), 0.85]) + rospy.loginfo("place: " + str(place)) + + # transform poses to find out position relative to robot + lt = LocalTransformer() + pose_in_robot_frame = lt.transform_pose(pose_in_map, robot.get_link_tf_frame("base_link")) + + if pose_in_robot_frame.pose.position.y > 0.25: + TalkingMotion("please take a seat to the left from me").perform() + # move pose more to the left for clear pointing pose + pose_in_robot_frame.pose.position.y += 0.4 + + elif pose_in_robot_frame.pose.position.y < -0.35: + TalkingMotion("please take a seat to the right from me").perform() + # move pose more to the right for clear pointing pose + pose_in_robot_frame.pose.position.y -= 0.4 + + else: + TalkingMotion("please take a seat in front of me").perform() + + # get pose in map + pose_in_map = lt.transform_pose(pose_in_robot_frame, "map") + free_seat = True + break + else: + rospy.loginfo("find free chairs") + for place in seat[1]: + if place[0] == 'chair': + if place[1] == 'False': + pose_in_map = Pose([float(place[2]), float(place[3]), 0.85]) + TalkingMotion("please take a seat on the free chair").perform() + free_seat = True + break + + if free_seat: + pose_guest = PointStamped() + pose_guest.header.frame_id = "map" + pose_guest.point.x = pose_in_map.pose.position.x + pose_guest.point.y = pose_in_map.pose.position.y + pose_guest.point.z = 0.85 + + MoveGripperMotion(GripperState.CLOSE, Arms.LEFT).perform() + PointingMotion(pose_guest).perform() + + rospy.loginfo("found seat") + return pose_guest + else: + TalkingMotion("no free seat detected").perform() + + return free_seat + + +def detect_host_face(host: HumanDescription): + """ + function to detect a face on the couch + :param host: variable the id gets stored in + """ + try: + + LookAtAction([look_couch]).resolve().perform() + human_dict = DetectAction(technique='human', state='face').resolve().perform() + id_humans = human_dict["keys"] + rospy.loginfo("found face of host") + host_pose = human_dict[id_humans[0]] + host.set_id(id_humans[0]) + host.set_pose(PoseStamped_to_Point(host_pose)) + return True + + except PerceptionObjectNotFound: + return False + + +def identify_faces(host: HumanDescription, guest1: HumanDescription): + """ + function to identify known faces on a location + :param host: object with ID of host, that is searched + :param guest1: object with ID of guest, that is searched + note that the name giving in based on Robocup Receptionist challenge. any two + HumanDescriptions can be used. we assume that only two humans are in the area + """ + LookAtAction([look_couch]).resolve().perform() + counter = 0 + found_guest = False + found_host = False + while True: + unknown = [] + try: + if counter > 4 or (found_guest and found_host): + break + + elif counter == 2: + TalkingMotion("please look at me").perform() + rospy.sleep(2.5) + + elif counter == 3: + # look to the side to find faces + MoveJointsMotion(["head_pan_joint"], [-0.3]).perform() + TalkingMotion("please look at me").perform() + rospy.sleep(2.5) + + human_dict = DetectAction(technique='human', state='face').resolve().perform() + rospy.loginfo("faces detect: " + str(human_dict)) + counter += 1 + id_humans = human_dict["keys"] + + # loop through detected face Ids + for key in id_humans: + # found guest + if key == guest1.id: + # update pose + guest1_pose = human_dict[guest1.id] + found_guest = True + guest1.set_pose(PoseStamped_to_Point(guest1_pose)) + + # found host + elif key == host.id: + # update pose + found_host = True + host_pose = human_dict[host.id] + host.set_pose(PoseStamped_to_Point(host_pose)) + else: + # store unknown ids for failure handling + unknown.append(key) + + except PerceptionObjectNotFound: + counter += 1 + if counter == 3: + MoveJointsMotion(["head_pan_joint"], [-0.3]).perform() + TalkingMotion("please look at me").perform() + rospy.sleep(2.5) + + # Failure Handling if at least one person was not recognized + if not found_guest and not found_host: + try: + # both have not been recognized chose randomly + guest1.set_pose(PoseStamped_to_Point(human_dict[unknown[0]])) + host.set_pose(PoseStamped_to_Point(human_dict[unknown[1]])) + except Exception as e: + print(e) + else: + # either guest or host was not found + # if one unknown face was detected, it has to be the second human looked for + if not found_guest: + try: + guest1.set_pose(PoseStamped_to_Point(human_dict[unknown[0]])) + except Exception as e: + print(e) + elif not found_host: + try: + host.set_pose(PoseStamped_to_Point(human_dict[unknown[0]])) + except Exception as e: + print(e) + + +def introduce(human1: HumanDescription, human2: HumanDescription): + """ + Text for robot to introduce two people to each other and alternate gaze + :param human1: the first human the robot talks to + :param human2: the second human the robot talks to + """ + + pub_pose = rospy.Publisher('/human_pose', PointStamped, queue_size=10) + rospy.sleep(2) + + if human1.pose: + pub_pose.publish(human1.pose) + rospy.sleep(1.0) + pub_pose.publish(human1.pose) + TalkingMotion(f"Hey, {human1.name}").perform() + rospy.sleep(2.5) + + if human2.pose: + pub_pose.publish(human2.pose) + rospy.sleep(1) + TalkingMotion(f" This is {human2.name} and their favorite drink is {human2.fav_drink}").perform() + rospy.sleep(2.2) + TalkingMotion(f"Hey, {human2.name}").perform() + rospy.sleep(2) + + if human1.pose: + pub_pose.publish(human1.pose) + rospy.sleep(1.5) + TalkingMotion(f" This is {human1.name} and their favorite drink is {human1.fav_drink}").perform() + + rospy.sleep(1) + + +def PoseStamped_to_Point(pose: PoseStamped): + """ + function to transform PoseStamped to PointStamped in '/map' frame + :param pose: pose to be transformed + """ + point_pose = PointStamped() + point_pose.header.frame_id = "map" + point_pose.point.x = pose.pose.position.x + point_pose.point.y = pose.pose.position.y + point_pose.point.z = pose.pose.position.z + + return point_pose + + +def describe(human: HumanDescription): + """ + HRI-function for describing a human more detailed. + the following will be stated: gender, headgear, clothing, brightness of clothes + :param human: human to be described + """ + pub_pose2 = rospy.Publisher('/human_pose', PointStamped, queue_size=10) + + if human.attributes != "False" and human.attributes is not None: + print(human.attributes) + + if human.pose: + pub_pose2.publish(human.pose) + + TalkingMotion(f"I will describe {human.name} further now").perform() + rospy.sleep(1.5) + + # gender + TalkingMotion(f"i think your gender is {human.attributes[0]}").perform() + rospy.sleep(1.5) + + # headgear or not + TalkingMotion(f"you are not wearing a hat").perform() + rospy.sleep(1) + + # kind of clothes + TalkingMotion(f"you are {human.attributes[2]}").perform() + rospy.sleep(1) + + # brightness of clothes + TalkingMotion(f"you are wearing {human.attributes[3]}").perform() + rospy.sleep(2.5) + TalkingMotion("have fun at the party").perform() + + + + diff --git a/src/pycram/designators/object_designator.py b/src/pycram/designators/object_designator.py index 54cf71817..3942dfd02 100644 --- a/src/pycram/designators/object_designator.py +++ b/src/pycram/designators/object_designator.py @@ -1,6 +1,7 @@ from __future__ import annotations import dataclasses +from typing import Optional import owlready2 import sqlalchemy.orm @@ -207,3 +208,59 @@ def __iter__(self): for world_obj in World.get_object_by_type(obj_desig.obj_type): obj_desig.world_object = world_obj yield obj_desig + + +class HumanDescription: + """ + Class that represents humans. this class does not spawn a human in a simulation. + """ + + def __init__(self, name: str, fav_drink: Optional = None, + pose: Optional = None, attributes: Optional = None): + """ + :param name: name of human + :param fav_drink: favorite drink of human + :param pose: last known pose of human + """ + self.name = name + self.fav_drink = fav_drink + self.pose = pose + self.attributes = attributes + self.id = -1 + + def set_id(self, new_id: int): + """ + function for changing id of human + is given by perception with face recognition + :param new_id: new id of human + """ + self.id = new_id + + def set_name(self, new_name): + """ + function for changing name of human + :param new_name: new name of human + """ + self.name = new_name + + def set_drink(self, new_drink): + """ + function for changing/setting favorite drink of human + :param new_drink: name of drink + """ + self.fav_drink = new_drink + + def set_pose(self, new_pose): + """ + function for changing pose of human + :param new_pose: new pose of human + """ + print("in set pose") + self.pose = new_pose + + def set_attributes(self, attribute_list): + """ + function for setting attributes + :param attribute_list: list with attributes: gender, headgear, kind of clothes, bright/dark clothes + """ + self.attributes = attribute_list \ No newline at end of file diff --git a/src/pycram/utilities/robocup_utils.py b/src/pycram/utilities/robocup_utils.py new file mode 100644 index 000000000..707c15030 --- /dev/null +++ b/src/pycram/utilities/robocup_utils.py @@ -0,0 +1,285 @@ +import actionlib +import rospy +from actionlib_msgs.msg import GoalStatusArray +from sensor_msgs.msg import LaserScan, JointState +from sound_play.msg import SoundRequestActionGoal, SoundRequest +from std_msgs.msg import Int32 +from tmc_control_msgs.msg import GripperApplyEffortActionGoal +from tmc_msgs.msg import Voice, TalkRequestAction, TalkRequestActionGoal +import pycram.external_interfaces.giskard as giskardpy +from pycram.designators.object_designator import * +from pycram.fluent import Fluent + + +def pakerino(torso_z=0.15, config=None): + if not config: + config = {'arm_lift_joint': torso_z, 'arm_flex_joint': 1, 'arm_roll_joint': -1.2, 'wrist_flex_joint': -1.5, + 'wrist_roll_joint': 0} + return giskardpy.achieve_joint_goal(config) + + + +class SoundRequestPublisher: + """ + A class to publish sound requests in a ROS environment. + """ + + current_subscriber: rospy.Subscriber = None + """ + Reference to the current subscriber instance. + """ + + def __init__(self, topic='/sound_play/goal', queue_size=10, latch=True): + """ + Initializes the SoundRequestPublisher with a ROS publisher. + + :param topic: The ROS topic to publish sound requests to. Default is '/sound_play/goal'. + :param queue_size: The size of the message queue for the publisher. Default is 10. + :param latch: Whether the publisher should latch messages. Default is True. + """ + self.pub = rospy.Publisher(topic, SoundRequestActionGoal, queue_size=queue_size, latch=latch) + self.msg = SoundRequestActionGoal() + self.msg.goal.sound_request.sound = 1 + self.msg.goal.sound_request.command = 1 + self.msg.goal.sound_request.volume = 2.0 + + def publish_sound_request(self): + """ + Publish the sound request message. + """ + rospy.loginfo("Publishing sound request") + rospy.loginfo("Waiting for subscribers to connect...") + while self.pub.get_num_connections() == 0: + rospy.sleep(0.1) # Sleep for 100ms and check again + self.pub.publish(self.msg) + rospy.loginfo("Sound request published") + + +class StartSignalWaiter: + """ + A class to wait for a start signal based on laser scan data in a ROS environment. + """ + + current_subscriber: rospy.Subscriber = None + """ + Reference to the current subscriber instance. + """ + + def __init__(self): + """ + Initializes the StartSignalWaiter with a Fluent object. + """ + self.fluent = Fluent() + self.current_subscriber = None + + def wait_for_startsignal(self): + """ + Subscribe to the laser scan topic and wait for the door to open before continuing. + """ + + def laser_scan_callback(msg): + ranges = list(msg.ranges) + if len(ranges) > 481 and ranges[481] > 0.5: + self.fluent.set_value(True) + rospy.loginfo("Door is open, unsubscribing from topic") + self.current_subscriber.unregister() + + rospy.loginfo("Waiting for starting signal.") + self.current_subscriber = rospy.Subscriber("hsrb/base_scan", LaserScan, laser_scan_callback) + + rospy.loginfo("Waiting for door to open") + self.fluent.wait_for() + + rospy.loginfo("Start signal received.") + + def something_in_the_way(self): + """ + Subscribe to the laser scan topic and wait for an obstacle to appear within a specified distance + before continuing. + """ + + def laser_scan_callback(msg): + ranges = list(msg.ranges) + rospy.loginfo(f"Received laser scan with {len(ranges)} ranges") + # Ensure there are enough elements in ranges + if len(ranges) > 501: + obstacle_detected = False + # Check if any value in the range 461 to 501 is smaller than 1.0 + for i in range(470, 510): + # print(ranges[i]) + if ranges[i] < 0.98: + obstacle_detected = True + rospy.loginfo(f"Obstacle detected at index {i} with range {ranges[i]}") + break + if obstacle_detected: + self.fluent.set_value(True) + rospy.loginfo("Obstacle detected within 1.0 meter, unsubscribing from topic") + self.current_subscriber.unregister() + return True + else: + self.fluent.set_value(True) + rospy.loginfo("No Obstacle detected within 1.0 meter, unsubscribing from topic") + self.current_subscriber.unregister() + return False + + rospy.loginfo("Waiting for obstacle detection signal.") + self.current_subscriber = rospy.Subscriber("hsrb/base_scan", LaserScan, laser_scan_callback) + + rospy.loginfo("Waiting for obstacle to be detected") + self.fluent.wait_for() + + rospy.loginfo("Obstacle detection signal received.") + + + + + + def update_ros_parameters(new_params): + """ + Update specified parameters on the ROS parameter server. + + :param new_params: Dictionary with new parameter values. + """ + try: + rospy.init_node('update_params_node', anonymous=True) + + for param, value in new_params.items(): + rospy.set_param(param, value) + + print("Parameters updated successfully.") + + except rospy.ROSInterruptException as e: + print(f"An error occurred: {e}") + + + +class TextToSpeechPublisher(): + + def __init__(self): + self.pub = rospy.Publisher('/talk_request_action/goal', TalkRequestActionGoal, queue_size=10) + self.status_sub = rospy.Subscriber('/talk_request_action/status', GoalStatusArray, self.status_callback) + self.status_list = [] + + def status_callback(self, msg): + self.status_list = msg.status_list + + def pub_now(self, sentence, talk_bool: bool = True, wait_bool: bool = True): + rospy.logerr("talking sentence: " + str(sentence)) + if talk_bool: + while not rospy.is_shutdown(): + if not self.status_list or not wait_bool: # Check if the status list is empty + goal_msg = TalkRequestActionGoal() + goal_msg.header.stamp = rospy.Time.now() + goal_msg.goal.data.language = 1 + goal_msg.goal.data.sentence = sentence + + while self.pub.get_num_connections() == 0: + rospy.sleep(0.1) + + self.pub.publish(goal_msg) + break + + + +class ImageSwitchPublisher: + """ + A class to publish image switch requests in a ROS environment. + """ + + def __init__(self, topic='/media_switch_topic', queue_size=10, latch=True): + """ + Initializes the ImageSwitchPublisher with a ROS publisher. + + :param topic: The ROS topic to publish image switch requests to. Default is '/image_switch_topic'. + :param queue_size: The size of the message queue for the publisher. Default is 10. + :param latch: Whether the publisher should latch messages. Default is True. + """ + self.pub = rospy.Publisher(topic, Int32, queue_size=queue_size, latch=latch) + self.msg = Int32() + + def pub_now(self, image_id): + """ + Publish the image switch request message. + + :param image_id: The ID of the image to switch to. + """ + self.msg.data = image_id + rospy.loginfo(f"Publishing image switch request with ID {image_id}") + rospy.loginfo("Waiting for subscribers to connect...") + while self.pub.get_num_connections() == 0: + rospy.sleep(0.1) # Sleep for 100ms and check again + self.pub.publish(self.msg) + rospy.loginfo("Image switch request published") + + +class GraspListener: + def __init__(self): + + # Subscribe to the joint_states topic + rospy.Subscriber("/hsrb/robot_state/joint_states", JointState, self.joint_states_callback) + + # Define the positions that indicate a grasp + # Define the positions that indicate a grasp + self.grasp_thresholds = { + "hand_r_distal_joint": (-1.3, 0.61), # Adjusted based on open and closed state data with offset + "hand_l_distal_joint": (-1.3, 0.61) + } + # Open + # State: + # hand_r_distal_joint: -1.477408 + # hand_l_distal_joint: -1.477408 + # Closed + # State: + # hand_r_distal_joint: 0.800545 + # hand_l_distal_joint: 0.782546 + + # Variable to store the grasp state + self.grasped = False + + def check_grasp(self): + print(self.grasped) + return self.grasped + + def joint_states_callback(self, msg): + # Extract the position of the relevant joints + try: + hand_r_index = msg.name.index("hand_r_distal_joint") + hand_l_index = msg.name.index("hand_l_distal_joint") + + hand_r_position = msg.position[hand_r_index] + hand_l_position = msg.position[hand_l_index] + + # Check if the positions are within the grasping thresholds + if (self.grasp_thresholds["hand_r_distal_joint"][0] <= hand_r_position <= + self.grasp_thresholds["hand_r_distal_joint"][1] and + self.grasp_thresholds["hand_l_distal_joint"][0] <= hand_l_position <= + self.grasp_thresholds["hand_l_distal_joint"][1]): + self.grasped = True + else: + self.grasped = False + + # rospy.loginfo("Grasp state: %s", "Grasped" if self.grasped else "Not grasped") + + except ValueError as e: + rospy.logerr("Joint names not found in joint_states message: %s", e) + +# Hints: List for image view (mit Zahlen ändert man das Bild) +# "hi.png" -> 0 +# "talk.png" -> 1 +# "dish.png" -> 2 +# "done.png" -> 3 +# "drop.png" -> 4 +# "handover.png" -> 5 +# "order.png" -> 6 +# "picking.png" -> 7 +# "placing.png" -> 8 +# "repeat.png" -> 9 +# "search.png" -> 10 +# "waving.mp4" -> 11 +# "following" -> 12 + + +# Example usage: +# image_switch_publisher = ImageSwitchPublisher() +# image_switch_publisher.publish_image_switch(12) +# Publishing and latching message. Press ctrl-C to terminate.