def button2_long(self): """ Logs that button 2 has been pressed long. """ self.get_logger().warn('Pause button (2) pressed long') speak("2 long", self.speak_publisher, speaking_active=self.speaking_active)
def set_pause(self, state): self.pause = state if state: text = "Pause!" else: text = "Unpause!" self.node.get_logger().warn(text) speak(text, self.speak_publisher, speaking_active=True, priority=90) self.pause_publisher.publish(Bool(data=state))
def button1_long(self): rospy.logwarn('Unpause button (1) pressed long') speak("1 long", self.speak_publisher, speaking_active=self.speaking_active) if self.foot_zero_mode: try: response = self.foot_zero_method() except rospy.ServiceException as exc: print("foot zeroing service did not process request: " + str(exc))
def button2_short(self): rospy.logwarn('Pause button (2) pressed short') speak("2 short", self.speak_publisher, speaking_active=self.speaking_active) if self.manual_penality_mode: # switch penalty state by calling service on motion try: response = self.manual_penalize_method(1) # penalize except rospy.ServiceException as exc: print("Penalize service did not process request: " + str(exc))
def set_pause(self, state): self.pause = state if state: text = "Pause!" else: text = "Unpause!" rospy.logwarn(text) speak(text, self.speak_publisher, speaking_active=self.talking, priority=Speak.HIGH_PRIORITY) self.pause_publisher.publish(Bool(state))
def button1_short(self): rospy.logwarn('Unpause button (1) pressed short') speak("1 short", self.speak_publisher, speaking_active=self.speaking_active) self.shoot_publisher.publish(Bool(True)) if self.manual_penality_mode: # switch penalty state by calling service on motion try: response = self.manual_penalize_method(0) # unpenalize except rospy.ServiceException as exc: print("Penalize service did not process request: " + str(exc))
def on_shutdown_hook(self): if not self.blackboard: return # we got external shutdown, tell it to the DSD, it will handle it self.blackboard.shut_down_request = True self.node.get_logger().warn( "You're stopping the HCM. The robot will sit down and power off its motors." ) speak("Stopping HCM", self.blackboard.speak_publisher, priority=50) # now wait for it finishing the shutdown procedure while not self.blackboard.current_state == RobotControlState.HCM_OFF: # we still have to update everything self.blackboard.current_time = self.node.get_clock().now() self.dsd.update() self.hcm_state_publisher.publish(self.blackboard.current_state) self.node.get_clock().sleep_for(Duration(seconds=0.01))
def button1_long(self): """ Zeroes foot sensors, if foot zero mode is true. """ self.get_logger().warn('Unpause button (1) pressed long') speak("1 long", self.speak_publisher, speaking_active=self.speaking_active) if self.foot_zero_method.service_is_ready(): self.foot_zero_method.call_async(1) # penalize else: speak("Foot zeroing failed", self.speak_publisher, speaking_active=self.speaking_active) self.get_logger().warn( "foot zeroing service did not process request.")
def on_shutdown_hook(self): if not self.blackboard: return # we got external shutdown, tell it to the DSD, it will handle it self.blackboard.shut_down_request = True rospy.logwarn( "You're stopping the HCM. The robot will sit down and power off its motors." ) speak("Stopping HCM", self.blackboard.speak_publisher, priority=Speak.HIGH_PRIORITY) # now wait for it finishing the shutdown procedure while not self.blackboard.current_state == STATE_HCM_OFF: # we still have to update everything self.blackboard.current_time = rospy.Time.now() self.dsd.update() self.hcm_state_publisher.publish(self.blackboard.current_state) rospy.sleep(0.1)
def button1_short(self): """ Penalizes the robot, if it is not penalized and manual penalty mode is true. """ self.get_logger().warn('Pause button (2) pressed short') speak("2 short", self.speak_publisher, speaking_active=self.speaking_active) if self.manual_penality_mode: # switch penalty state by calling service on motion if self.manual_penalize_method.service_is_ready(): self.manual_penalize_method.call_async(1) # penalize else: speak("Pause failed", self.speak_publisher, speaking_active=self.speaking_active) self.get_logger().warn( "Penalize service did not process request.")
def perform(self, reevaluate=False): if self.blackboard.visualization_active: return "ON_GROUND" # check if the robot is currently being picked up. foot have no connection to the ground, # but robot is more or less upright (to differentiate from falling) if self.blackboard.pressure_sensors_installed and not self.blackboard.simulation_active and \ sum(self.blackboard.pressures) < 10 and \ abs(self.blackboard.smooth_accel[0]) < self.blackboard.pickup_accel_threshold and \ abs(self.blackboard.smooth_accel[1]) < self.blackboard.pickup_accel_threshold: self.blackboard.current_state = RobotControlState.PICKED_UP if not reevaluate: speak("Picked up!", self.blackboard.speak_publisher, priority=50) # we do an action sequence to go to walkready and stay in picked up state return "PICKED_UP" # robot is not picked up return "ON_GROUND"
def __init__(self): rclpy.init(args=None) self.node = Node("hcm", allow_undeclared_parameters=True, automatically_declare_parameters_from_overrides=True) multi_executor = MultiThreadedExecutor() multi_executor.add_node(self.node) self.spin_thread = threading.Thread(target=multi_executor.spin, args=(), daemon=True) #self.spin_thread = threading.Thread(target=rclpy.spin, args=(self.node,), daemon=True) self.spin_thread.start() self.blackboard = None # --- Initialize Node --- # Otherwise messages will get lost, bc the init is not finished self.node.get_clock().sleep_for(Duration(seconds=0.1)) self.node.get_logger().debug("Starting hcm") self.simulation_active = self.node.get_parameter("simulation_active") # dsd self.blackboard = HcmBlackboard(self.node) self.blackboard.animation_action_client = ActionClient( self.node, PlayAnimation, 'animation') self.blackboard.dynup_action_client = ActionClient( self.node, Dynup, 'dynup') dirname = os.path.dirname(os.path.realpath(__file__)) + "/hcm_dsd" self.dsd = DSD(self.blackboard, "debug/dsd/hcm", node=self.node) self.dsd.register_actions(os.path.join(dirname, 'actions')) self.dsd.register_decisions(os.path.join(dirname, 'decisions')) self.dsd.load_behavior(os.path.join(dirname, 'hcm.dsd')) self.hcm_deactivated = False # Publisher / subscriber self.joint_goal_publisher = self.node.create_publisher( JointCommand, 'DynamixelController/command', 1) self.hcm_state_publisher = self.node.create_publisher( RobotControlState, 'robot_state', 1) # todo latch self.blackboard.speak_publisher = self.node.create_publisher( Audio, 'speak', 1) # important to make sure the connection to the speaker is established, for next line self.node.get_clock().sleep_for(Duration(seconds=0.1)) speak("Starting hcm", self.blackboard.speak_publisher, priority=50) self.node.create_subscription(Imu, "imu/data", self.update_imu, 1) self.node.create_subscription(FootPressure, "foot_pressure_left/filtered", self.update_pressure_left, 1) self.node.create_subscription(FootPressure, "foot_pressure_right/filtered", self.update_pressure_right, 1) self.node.create_subscription(JointCommand, "walking_motor_goals", self.walking_goal_callback, 1) self.node.create_subscription(AnimationMsg, "animation", self.animation_callback, 1) self.node.create_subscription(JointCommand, "dynup_motor_goals", self.dynup_callback, 1) self.node.create_subscription(JointCommand, "head_motor_goals", self.head_goal_callback, 1) self.node.create_subscription(JointCommand, "record_motor_goals", self.record_goal_callback, 1) self.node.create_subscription(JointCommand, "kick_motor_goals", self.kick_goal_callback, 1) self.node.create_subscription(Bool, "pause", self.pause, 1) self.node.create_subscription(JointState, "joint_states", self.joint_state_callback, 1) self.node.create_subscription(PointStamped, "cop_l", self.cop_l_cb, 1) self.node.create_subscription(PointStamped, "cop_r", self.cop_r_cb, 1) self.node.create_subscription(Bool, "core/power_switch_status", self.power_cb, 1) self.node.create_subscription(Bool, "hcm_deactivate", self.deactivate_cb, 1) self.node.create_subscription(DiagnosticArray, "diagnostics_agg", self.blackboard.diag_cb, 1) self.node.add_on_set_parameters_callback(self.on_set_parameters) self.main_loop()
def __init__(self): # necessary for on shutdown hook, incase of direct shutdown before finished initilization self.blackboard = None # --- Initialize Node --- log_level = rospy.DEBUG if rospy.get_param("debug_active", False) else rospy.INFO rospy.init_node('bitbots_hcm', log_level=log_level, anonymous=False) rospy.sleep( 0.1 ) # Otherwise messages will get lost, bc the init is not finished rospy.loginfo("Starting hcm") rospy.on_shutdown(self.on_shutdown_hook) # stack machine self.blackboard = HcmBlackboard() self.blackboard.animation_action_client = actionlib.SimpleActionClient( 'animation', PlayAnimationAction) self.blackboard.dynup_action_client = actionlib.SimpleActionClient( 'dynup', DynUpAction) dirname = os.path.dirname(os.path.realpath(__file__)) + "/hcm_dsd" self.dsd = DSD(self.blackboard, "/debug/dsd/hcm") self.dsd.register_actions(os.path.join(dirname, 'actions')) self.dsd.register_decisions(os.path.join(dirname, 'decisions')) self.dsd.load_behavior(os.path.join(dirname, 'hcm.dsd')) # Publisher / subscriber self.joint_goal_publisher = rospy.Publisher( 'DynamixelController/command', JointCommand, queue_size=1) self.hcm_state_publisher = rospy.Publisher('robot_state', RobotControlState, queue_size=1, latch=True) self.blackboard.speak_publisher = rospy.Publisher('speak', Speak, queue_size=1) rospy.sleep( 0.1 ) # important to make sure the connection to the speaker is established, for next line speak("Starting hcm", self.blackboard.speak_publisher, priority=Speak.HIGH_PRIORITY) rospy.Subscriber("imu/data", Imu, self.update_imu, queue_size=1, tcp_nodelay=True) rospy.Subscriber("foot_pressure", FootPressure, self.update_pressure, queue_size=1, tcp_nodelay=True) rospy.Subscriber("walking_motor_goals", JointCommand, self.walking_goal_callback, queue_size=1, tcp_nodelay=True) rospy.Subscriber("animation", AnimationMsg, self.animation_callback, queue_size=1, tcp_nodelay=True) rospy.Subscriber("animation_motor_goals", JointCommand, self.dynup_callback, queue_size=1, tcp_nodelay=True) rospy.Subscriber("head_motor_goals", JointCommand, self.head_goal_callback, queue_size=1, tcp_nodelay=True) rospy.Subscriber("record_motor_goals", JointCommand, self.record_goal_callback, queue_size=1, tcp_nodelay=True) rospy.Subscriber("kick_motor_goals", JointCommand, self.kick_goal_callback, queue_size=1, tcp_nodelay=True) rospy.Subscriber("pause", Bool, self.pause, queue_size=1, tcp_nodelay=True) rospy.Subscriber("joint_states", JointState, self.joint_state_callback, queue_size=1, tcp_nodelay=True) self.dyn_reconf = Server(hcm_paramsConfig, self.reconfigure) self.main_loop()
def button2_long(self): rospy.logwarn('Pause button (2) pressed long') speak("2 long", self.speak_publisher, speaking_active=self.speaking_active)