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: