def run(dsd):
"""
Main run-loop
:returns: Never
"""
node = Node("head_node")
rate = node.create_rate(60)
while rclpy.ok():
dsd.update()
rate.sleep()
# Also stop cpp node
roscpp_shutdown()
def run():
node = Node("testHeadBehaviour")
pub_ball = node.create_publisher(BallInImage, "ball_in_image", 1)
pub_hmg = node.create_publisher(JointTrajectory, "head_motor_goals", 1)
hmg = JointTrajectory()
goal = JointTrajectoryPoint()
goal.positions = [0, 0]
goal.velocities = [0, 0]
hmg.points = [goal]
counter = 320
direction = 1
node.get_logger().info("Create Test")
rclpy.init(args=None)
pub_hmg.publish(hmg)
rate = node.create_rate(4)
node.get_logger().debug("Laeuft...")
while rclpy.ok():
# Ball in Image
ball = BallInImage()
ball.center.x = counter
if(counter > 340 or counter < 300):
direction *= -1
counter += direction
else:
counter += direction
ball.center.y = 200
ball.diameter = 10
ball.confidence = 1
balls = BallInImageArray()
balls.candidates.append(ball)
pub_ball.publish(balls)
node.get_logger().info("Published ball: %s" % counter)
rate.sleep()
def reset_leds():
global leds_red, previous_req
leds_red = False
led_serv.call(previous_req)
# wait a moment on startup, otherwise we will think there is a problem while ros control is still booting
set_leds_service = node.create_client(Leds, "/set_leds")
set_leds_service.wait_for_service()
node.get_clock().sleep_for(Duration(seconds=1))
node.create_subscription(DiagnosticStatus, "/diagnostics_toplevel_state", cb,
1)
rate = node.create_rate(100)
while rclpy.ok():
if last_hardware_error_time is not None:
current_time = node.get_clock().now().to_sec()
if currently_blinking:
# we are currently blinking, check if the last hardware error is to long ago
if current_time - last_hardware_error_time > ERROR_TIMEOUT:
currently_blinking = False
reset_leds()
led_set_time = current_time
else:
if leds_red and current_time - led_set_time > BLINK_DURATION:
# reset LEDs
reset_leds()
led_set_time = current_time
elif not leds_red and current_time - led_set_time > BLINK_DURATION:
