#!/usr/bin/env python3 from abc import ABC, abstractmethod from time import sleep import numpy as np import rospy import smach import smach_ros from larcc_demos.srv import ActivateStateOutcome, ActivateStateOutcomeRequest, ActivateStateOutcomeResponse from states.s1 import S1_FastObjectManipulation from states.s2 import S2_SafeObjectManipulation from states.s3 import S3_GiveObject from states.s4 import S4_WaitForInteraction from states.s5 import S5_WaitToRecoverObject from states.s6 import S6_RecoverObject # main def main(): rospy.init_node('larcc_demo_sm') # Create a SMACH state machine sm = smach.StateMachine(outcomes=['e99_finish_demo']) sm.userdata.object = None sm.userdata.last_goal_id = 0 sm.userdata.positions = rospy.get_param("/demo/positions") sm.userdata.last_position_id = 0 max_vel_wrist_1 = rospy.get_param("/robot_description_planning/joint_limits/wrist_1_joint/max_velocity") max_vel_wrist_2 = rospy.get_param("/robot_description_planning/joint_limits/wrist_2_joint/max_velocity") max_vel_wrist_3 = rospy.get_param("/robot_description_planning/joint_limits/wrist_3_joint/max_velocity") max_vel_shoulder_pan = rospy.get_param("/robot_description_planning/joint_limits/shoulder_pan_joint/max_velocity") max_vel_shoulder_lift = rospy.get_param("/robot_description_planning/joint_limits/shoulder_lift_joint/max_velocity") max_vel_elbow = rospy.get_param("/robot_description_planning/joint_limits/elbow_joint/max_velocity") sm.userdata.max_joint_values = np.array([max_vel_elbow, max_vel_shoulder_lift, max_vel_shoulder_pan, max_vel_wrist_1, max_vel_wrist_2, max_vel_wrist_3]) waypoint_vel = 0.9 max_vel = 0.7 fa = 0.6 # Approach factor sm.userdata.movement_order = [ # Move ball in pose 1 to pose 2 # {"pos": ["ball_pose_1_app"], "max_vel": factor_app * max_vel, "way_point_vel": 0, "object": None}, {"pos": ["ball_pose_1_app", "ball_pose_1"], "max_vel": [max_vel, fa * max_vel], "way_point_vel": [fa * waypoint_vel], "object": None}, # Grab ball in pose 1 {"pos": ["close"], "max_vel": max_vel, "way_point_vel": 0, "object": None}, # Grab ball in pose 1 {"pos": ["ball_pose_1_app"], "max_vel": [fa * max_vel], "way_point_vel": [0], "object": 0}, # Lift ball in pose 1 # {"pos": ["waypoint", "ball_pose_2_app"], "max_vel": max_vel, "way_point_vel": waypoint_vel, "object": 0}, # Go to ball pose 2 {"pos": ["waypoint", "ball_pose_2_app", "ball_pose_2"], "max_vel": [max_vel, max_vel, fa * max_vel], "way_point_vel": [waypoint_vel, fa * waypoint_vel], "object": 0}, # Lower ball in pose 2 {"pos": ["open"], "max_vel": max_vel, "way_point_vel": 0, "object": None}, # Leave ball in pose 2 {"pos": ["ball_pose_2_app"], "max_vel": [fa * max_vel], "way_point_vel": [0], "object": None}, # Go up in ball in pose 2 # Move block in pose 1 to pose 2 {"pos": ["waypoint", "block_pose_1_app", "block_pose_1"], "max_vel": [max_vel, max_vel, fa * max_vel], "way_point_vel": [waypoint_vel,fa * waypoint_vel], "object": None}, # Grab block in pose 1 # {"pos": ["block_pose_1"], "max_vel": factor_app * max_vel, "way_point_vel": 0, "object": None}, # lower to block in pose 1 {"pos": ["close"], "max_vel": max_vel, "way_point_vel": 0, "object": None}, # Grab block in pose 1 {"pos": ["block_pose_1_app"], "max_vel": [fa * max_vel], "way_point_vel": [0], "object": 1}, # Lift block in pose 1 {"pos": ["waypoint", "block_pose_2_app", "block_pose_2"], "max_vel": [max_vel, max_vel, fa * max_vel], "way_point_vel": [waypoint_vel, fa * waypoint_vel], "object": 1}, # Go to block pose 2 # {"pos": ["block_pose_2"], "max_vel": factor_app * max_vel, "way_point_vel": 0, "object": 1}, # Lower block in pose 2 {"pos": ["open"], "max_vel": max_vel, "way_point_vel": 0, "object": None}, # Leavdefine_movemente block in pose 2 {"pos": ["block_pose_2_app"], "max_vel": [fa * max_vel], "way_point_vel": [0], "object": None}, # Go up in block in pose 2 # Move ball in pose 2 to pose 1 {"pos": ["ball_pose_2_app", "ball_pose_2"], "max_vel": [max_vel, fa * max_vel], "way_point_vel": [waypoint_vel], "object": None}, # Grab ball in pose 2 {"pos": ["close"], "max_vel": max_vel, "way_point_vel": 0, "object": None}, # Grab ball in pose 2 {"pos": ["ball_pose_2_app"], "max_vel": [fa * max_vel], "way_point_vel": [0], "object": 0}, # Lift ball in pose 2 {"pos": ["waypoint", "ball_pose_1_app", "ball_pose_1"], "max_vel":[max_vel, max_vel, fa * max_vel], "way_point_vel": [waypoint_vel, fa * waypoint_vel], "object": 0}, # Go to ball pose 2 # {"pos": ["ball_pose_1"], "max_vel": factor_app * max_vel, "way_point_vel": 0, "object": 0}, # Lower ball in pose 1 {"pos": ["open"], "max_vel": max_vel, "way_point_vel": 0, "object": None}, # Leave ball in pose 1 {"pos": ["ball_pose_1_app"], "max_vel": [fa * max_vel], "way_point_vel": [0], "object": None}, # Go up in ball in pose 1 # Move block in pose 2 to pose 1 {"pos": ["waypoint", "block_pose_2_app", "block_pose_2"], "max_vel": [max_vel, max_vel, fa * max_vel], "way_point_vel": [waypoint_vel, fa * waypoint_vel], "object": None}, # Grab block in pose 2 # {"pos": ["block_pose_2"], "max_vel": factor_app * max_vel, "way_point_vel": 0, "object": None}, # lower to block in pose 2 {"pos": ["close"], "max_vel": max_vel, "way_point_vel": 0, "object": None}, # Grab block in pose 2 {"pos": ["block_pose_2_app"], "max_vel": [fa * max_vel], "way_point_vel": [0], "object": 1}, # Lift block in pose 2 {"pos": ["waypoint", "block_pose_1_app", "block_pose_1"], "max_vel": [max_vel, max_vel, fa * max_vel], "way_point_vel": [waypoint_vel,fa * waypoint_vel], "object": 1}, # Go to block pose 2 # {"pos": ["block_pose_1"], "max_vel": fa * max_vel, "way_point_vel": 0, "object": 1}, # Lower block in pose 1 {"pos": ["open"], "max_vel": max_vel, "way_point_vel": 0, "object": None}, # Leave block in pose 1 {"pos": ["block_pose_1_app"], "max_vel": [fa * max_vel], "way_point_vel": [0], "object": None}, # Go up in block in pose 1 ] # Open the container with sm: # Add states to the container smach.StateMachine.add('S1_FastObjectManipulation', S1_FastObjectManipulation(), transitions={'e1_operator_detected':'S2_SafeObjectManipulation'}) smach.StateMachine.add('S2_SafeObjectManipulation', S2_SafeObjectManipulation(), transitions={'e2_operator_not_detected':'S1_FastObjectManipulation', 'e3_gesture_a_detected':'S3_GiveObject'}) smach.StateMachine.add('S3_GiveObject', S3_GiveObject(), transitions={'e9_object_available':'S4_WaitForInteraction'}) smach.StateMachine.add('S4_WaitForInteraction', S4_WaitForInteraction(), transitions={'e5_interaction_pull_detected': 'S5_WaitToRecoverObject', 'e6_interaction_push_detected': 'S2_SafeObjectManipulation', 'e7_interaction_twist_detected': 'S3_GiveObject', 'e2_operator_not_detected':'S1_FastObjectManipulation'}) smach.StateMachine.add('S5_WaitToRecoverObject', S5_WaitToRecoverObject(), transitions={'e8_gesture_l_detected': 'S6_RecoverObject'}) smach.StateMachine.add('S6_RecoverObject', S6_RecoverObject(), transitions={'e10_object_recovered': 'S2_SafeObjectManipulation'}) # Create and start the introspection server sis = smach_ros.IntrospectionServer('server_name', sm, '/S0_Start') sis.start() # Execute SMACH plan outcome = sm.execute() sis.stop() if __name__ == '__main__': main()