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)