Skip to content

Commit

Permalink
(Hopefully) Runnable with 3d input.
Browse files Browse the repository at this point in the history
  • Loading branch information
Benteng Ma committed Feb 5, 2024
1 parent 98535a0 commit ecd469e
Show file tree
Hide file tree
Showing 2 changed files with 50 additions and 13 deletions.
38 changes: 37 additions & 1 deletion skills/src/lasr_skills/describe_people.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,41 @@

from .vision import GetImage, ImageMsgToCv2, Get3DImage, PclMsgToCv2, Get2DAnd3DImages

import actionlib
import numpy as np
# from sensor_msgs.msg import PointCloud2
# from control_msgs.msg import PointHeadAction, PointHeadGoal
# from geometry_msgs.msg import PointStamped

def point_head_client(xyz_array, u, v):
# rospy.init_node('point_head_client')

rospy.logwarn('making client')
client = actionlib.SimpleActionClient('/head_controller/point_head_action', PointHeadAction)
client.wait_for_server()

target_point = xyz_array[v, u]

point_camera = PointStamped()
point_camera.header.frame_id = "xtion_rgb_optical_frame"
point_camera.header.stamp = rospy.Time.now()
point_camera.point.x = target_point[0]
point_camera.point.y = target_point[1]
point_camera.point.z = target_point[2]

goal = PointHeadGoal()
goal.target = point_camera
goal.max_velocity = 1.0
# goal.min_duration = rospy.Duration(1.0)
goal.pointing_frame = "/head_2_link"
goal.pointing_axis.x = 1.0
goal.pointing_axis.y = 0.0
goal.pointing_axis.z = 0.0

rospy.logwarn('sending the goal and waiting')
client.send_goal_and_wait(goal)
rospy.logwarn('end')


class DescribePeople(smach.StateMachine):

Expand Down Expand Up @@ -99,8 +134,9 @@ def execute(self, userdata):
neck_coord = (int(result.poses[0].coord[0]), int(result.poses[0].coord[1]))
rospy.loginfo("COORD_XY:::%s" % str(neck_coord))
xyz = userdata.xyz
xyz = np.nanmean(xyz, axis=2)
# xyz = np.nanmean(xyz, axis=2)
rospy.loginfo("COORD_Z:::%s" % str(xyz[neck_coord[0]][neck_coord[1]]))
# point_head_client(xyz, neck_coord[0], neck_coord[1])
return 'succeeded'
except rospy.ServiceException as e:
rospy.logwarn(f"Unable to perform inference. ({str(e)})")
Expand Down
25 changes: 13 additions & 12 deletions tasks/receptionist/src/receptionist/sm.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,15 +22,16 @@ def __init__(self):
self.userdata.area_polygon = [[1.94, 0.15], [2.98, 0.28], [3.08, -0.68], [2.06, -0.84]]
self.userdata.depth_topic = "/xtion/depth_registered/points"
with self:
# smach.StateMachine.add('START', Start(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'})
# smach.StateMachine.add("GO_TO_WAIT_FOR_PERSON",GoToWaitForPerson(self.default), transitions={'succeeded': 'WAIT_FOR_PERSON'})
# smach.StateMachine.add('WAIT_FOR_PERSON', WaitForPersonInArea() ,transitions={'succeeded' : 'GO_TO_PERSON', 'failed' : 'failed'})
# smach.StateMachine.add('GO_TO_PERSON', GoToPerson(self.default),transitions={'succeeded':'ASK_FOR_NAME'})
# smach.StateMachine.add('ASK_FOR_NAME', AskForName(self.default),transitions={'failed':'ASK_FOR_NAME','succeeded':'ASK_FOR_DRINK'})
# smach.StateMachine.add('ASK_FOR_DRINK', AskForDrink(self.default),transitions={'failed':'ASK_FOR_DRINK','succeeded':'DESCRIBE_PEOPLE'})
# smach.StateMachine.add('DESCRIBE_PEOPLE', DescribePeople(),transitions={'succeeded':'SPEAK_DESCRIPTIONS','failed':'GO_TO_SEATING_AREA'})
# smach.StateMachine.add('SPEAK_DESCRIPTIONS', SpeakDescriptions(self.default), transitions={'failed':'GO_TO_SEATING_AREA','succeeded':'GO_TO_SEATING_AREA'})
# smach.StateMachine.add('GO_TO_SEATING_AREA', GoToSeatingArea(self.default), transitions={'succeeded' : 'LOOK_FOR_SEATS'})
# smach.StateMachine.add('LOOK_FOR_SEATS', LookForSeats(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'})
smach.StateMachine.add('DESCRIBE_PEOPLE', DescribePeople(),transitions={'succeeded':'SPEAK_DESCRIPTIONS','failed':'DESCRIBE_PEOPLE'})
smach.StateMachine.add('SPEAK_DESCRIPTIONS', SpeakDescriptions(self.default), transitions={'failed':'DESCRIBE_PEOPLE','succeeded':'DESCRIBE_PEOPLE'})
smach.StateMachine.add('START', Start(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'})
smach.StateMachine.add("GO_TO_WAIT_FOR_PERSON",GoToWaitForPerson(self.default), transitions={'succeeded': 'WAIT_FOR_PERSON'})
smach.StateMachine.add('WAIT_FOR_PERSON', WaitForPersonInArea() ,transitions={'succeeded' : 'GO_TO_PERSON', 'failed' : 'failed'})
smach.StateMachine.add('GO_TO_PERSON', GoToPerson(self.default),transitions={'succeeded':'ASK_FOR_NAME'})
smach.StateMachine.add('ASK_FOR_NAME', AskForName(self.default),transitions={'failed':'ASK_FOR_NAME','succeeded':'ASK_FOR_DRINK'})
smach.StateMachine.add('ASK_FOR_DRINK', AskForDrink(self.default),transitions={'failed':'ASK_FOR_DRINK','succeeded':'DESCRIBE_PEOPLE'})
smach.StateMachine.add('DESCRIBE_PEOPLE', DescribePeople(),transitions={'succeeded':'SPEAK_DESCRIPTIONS','failed':'GO_TO_SEATING_AREA'})
smach.StateMachine.add('SPEAK_DESCRIPTIONS', SpeakDescriptions(self.default), transitions={'failed':'GO_TO_SEATING_AREA','succeeded':'GO_TO_SEATING_AREA'})
smach.StateMachine.add('GO_TO_SEATING_AREA', GoToSeatingArea(self.default), transitions={'succeeded' : 'LOOK_FOR_SEATS'})
smach.StateMachine.add('LOOK_FOR_SEATS', LookForSeats(self.default), transitions={'succeeded' : 'GO_TO_WAIT_FOR_PERSON'})
smach.StateMachine.add('END', End(self.default),transitions={'succeeded':'succeeded'})
# smach.StateMachine.add('DESCRIBE_PEOPLE', DescribePeople(),transitions={'succeeded':'SPEAK_DESCRIPTIONS','failed':'DESCRIBE_PEOPLE'})
# smach.StateMachine.add('SPEAK_DESCRIPTIONS', SpeakDescriptions(self.default), transitions={'failed':'DESCRIBE_PEOPLE','succeeded':'DESCRIBE_PEOPLE'})

0 comments on commit ecd469e

Please sign in to comment.