def head_look_for_ball(self):
# Set ball search head mode
head_mode = HeadMode()
head_mode.headMode = 0
self.pub_head_mode.publish(head_mode)
def head_look_forwards(self):
# Set front head mode
head_mode = HeadMode()
head_mode.headMode = 7
self.pub_head_mode.publish(head_mode)
def __init__(self):
# type: () -> None
"""
Initiate VisualCompassHandler
return: None
"""
# Init ROS package
rospack = rospkg.RosPack()
self.package_path = rospack.get_path('bitbots_visual_compass')
rospy.init_node('bitbots_visual_compass_setup')
rospy.loginfo('Initializing visual compass setup')
self.bridge = CvBridge()
self.config = {}
self.image_msg = None
self.compass = None
self.hostname = socket.gethostname()
# TODO: docs
self.base_frame = 'base_footprint'
self.camera_frame = 'camera_optical_frame'
self.tf_buffer = tf2.Buffer(cache_time=rospy.Duration(50))
self.listener = tf2.TransformListener(self.tf_buffer)
self.pub_head_mode = rospy.Publisher('head_mode',
HeadMode,
queue_size=1)
# Register VisualCompassConfig server for dynamic reconfigure and set callback
Server(VisualCompassConfig, self.dynamic_reconfigure_callback)
rospy.logwarn("------------------------------------------------")
rospy.logwarn("|| WARNING ||")
rospy.logwarn("||Please remove the LAN cable from the Robot, ||")
rospy.logwarn("||after pressing 'YES' you have 10 Seconds ||")
rospy.logwarn("||until the head moves OVER the LAN port!!! ||")
rospy.logwarn("------------------------------------------------\n\n")
try:
input = raw_input
except NameError:
pass
accept = input("Do you REALLY want to start? (YES/n)")
if accept == "YES":
rospy.logwarn("REMOVE THE LAN CABLE NOW!!!!!")
rospy.sleep(10)
head_mode = HeadMode()
head_mode.headMode = 10
self.pub_head_mode.publish(head_mode)
rospy.loginfo("Head mode has been set!")
rospy.spin()
else:
rospy.signal_shutdown(
"You aborted the process! Shuting down correctly.")
def set_head_duty(self, head_duty):
head_duty_msg = HeadMode()
head_duty_msg.headMode = head_duty
self.head_pub.publish(head_duty_msg)
def __init__(self):
super().__init__('joy_node')
self.declare_parameter('type', "noname")
self.declare_parameter('head', False)
for controller_type in ["noname", "xbox"]:
self.declare_parameter(f'{controller_type}.walking.gain_x', 0.0)
self.declare_parameter(f'{controller_type}.walking.stick_x', 0)
self.declare_parameter(f'{controller_type}.walking.gain_y', 0.0)
self.declare_parameter(f'{controller_type}.walking.stick_y', 0)
self.declare_parameter(f'{controller_type}.walking.stick_left', 0)
self.declare_parameter(f'{controller_type}.walking.stick_right', 0)
self.declare_parameter(f'{controller_type}.walking.duo_turn', False)
self.declare_parameter(f'{controller_type}.walking.gain_turn', 0.0)
self.declare_parameter(f'{controller_type}.head.gain_tilt', 0.0)
self.declare_parameter(f'{controller_type}.head.stick_tilt', 0)
self.declare_parameter(f'{controller_type}.head.gain_pan', 0.0)
self.declare_parameter(f'{controller_type}.head.stick_pan', 0)
self.declare_parameter(f'{controller_type}.kick.btn_left', 0)
self.declare_parameter(f'{controller_type}.kick.btn_right', 0)
self.declare_parameter(f'{controller_type}.misc.btn_cheering', 0)
selected_controller_type = self.get_parameter("type").get_parameter_value().string_value
self.config = {}
self.config["walking"] = {
"gain_x": self.get_parameter(f"{selected_controller_type}.walking.gain_x").get_parameter_value().double_value,
"stick_x": self.get_parameter(f"{selected_controller_type}.walking.stick_x").get_parameter_value().integer_value,
"gain_y": self.get_parameter(f"{selected_controller_type}.walking.gain_y").get_parameter_value().double_value,
"stick_y": self.get_parameter(f"{selected_controller_type}.walking.stick_y").get_parameter_value().integer_value,
"stick_right": self.get_parameter(f"{selected_controller_type}.walking.stick_right").get_parameter_value().integer_value,
"stick_left": self.get_parameter(f"{selected_controller_type}.walking.stick_left").get_parameter_value().integer_value,
"duo_turn": self.get_parameter(f"{selected_controller_type}.walking.duo_turn").get_parameter_value().bool_value,
"gain_turn": self.get_parameter(f"{selected_controller_type}.walking.gain_turn").get_parameter_value().double_value,
}
self.config["head"] = {
"gain_tilt": self.get_parameter(f"{selected_controller_type}.head.gain_tilt").get_parameter_value().double_value,
"stick_tilt": self.get_parameter(f"{selected_controller_type}.head.stick_tilt").get_parameter_value().integer_value,
"gain_pan": self.get_parameter(f"{selected_controller_type}.head.gain_pan").get_parameter_value().double_value,
"stick_pan": self.get_parameter(f"{selected_controller_type}.head.stick_pan").get_parameter_value().integer_value,
}
self.config["kick"] = {
"btn_left": self.get_parameter(f"{selected_controller_type}.kick.btn_left").get_parameter_value().integer_value,
"btn_right": self.get_parameter(f"{selected_controller_type}.kick.btn_right").get_parameter_value().integer_value,
}
self.config["misc"] = {
"btn_cheering": self.get_parameter(f"{selected_controller_type}.misc.btn_cheering").get_parameter_value().integer_value,
}
print(self.config)
# --- Initialize Topics ---
self.create_subscription(Joy,"joy", self.joy_cb, 1)
self.speak_pub = self.create_publisher(Audio, 'speak', 1)
self.speak_msg = Audio()
self.speak_msg.priority = 1
self.walk_publisher = self.create_publisher(Twist, 'cmd_vel', 1)
self.walk_msg = Twist()
self.last_walk_msg = Twist()
self.walk_msg.linear.x = 0.0
self.walk_msg.linear.y = 0.0
self.walk_msg.linear.z = 0.0
self.walk_msg.angular.x = 0.0
self.walk_msg.angular.y = 0.0
self.walk_msg.angular.z = 0.0
self.head_pub = self.create_publisher(JointCommand, "head_motor_goals", 1)
self.head_mode_pub = self.create_publisher(HeadMode, "head_mode", 1)
self.head_msg = JointCommand()
self.head_msg.max_currents = [-1.0] * 2
self.head_msg.velocities = [5.0] * 2
self.head_msg.accelerations = [40.0] * 2
self.head_modes = HeadMode()
def set_head_mode(self, mode):
msg = HeadMode()
msg.head_mode = mode
self.head_mode_pub.publish(msg)