find_package(sensor_msgs REQUIRED)
find_package(std_msgs REQUIRED)

rosidl_generate_interfaces(${PROJECT_NAME}
  "action/Fibonacci.action"
  "action/ConnectToChargingDock.action"
  DEPENDENCIES sensor_msgs std_msgs 
)

<depend>sensor_msgs</depend>
<depend>std_msgs</depend>

#!/usr/bin/env python3 
"""
Description:
  Action Server to connect to a battery charging dock.
  Action is successful when the power_supply_status goes from
  NOT_CHARGING(i.e. 3) to CHARGING(i.e. 1)
-------
Subscription Topics:
  Current battery state
  /battery_status – sensor_msgs/BatteryState
-------
Publishing Topics:
  Velocity command to navigate to the charging dock.
  /cmd_vel - geometry_msgs/Twist
-------
Author: Addison Sears-Collins
Website: AutomaticAddison.com
Date: November 12, 2021
"""
import time

# Import our action definition
from action_tutorials_interfaces.action import ConnectToChargingDock

# ROS Client Library for Python
import rclpy

# ActionServer library for ROS 2 Python
from rclpy.action import ActionServer

# Enables publishers, subscribers, and action servers to be in a single node
from rclpy.executors import MultiThreadedExecutor

# Handles the creation of nodes
from rclpy.node import Node

# Handle Twist messages, linear and angular velocity
from geometry_msgs.msg import Twist

# Handles BatteryState message
from sensor_msgs.msg import BatteryState

# Holds the current state of the battery
this_battery_state = BatteryState()

class ConnectToChargingDockActionServer(Node):
  """
  Create a ConnectToChargingDockActionServer class, 
  which is a subclass of the Node class.
  """
  def __init__(self):
  
    # Initialize the class using the constructor
    super().__init__('connect_to_charging_dock_action_server')
    
    # Instantiate a new action server
    # self, type of action, action name, callback function for executing goals
    self._action_server = ActionServer(
      self,
      ConnectToChargingDock,
      'connect_to_charging_dock',
      self.execute_callback)
      
    # Create a publisher
    # This node publishes the desired linear and angular velocity of the robot
    self.publisher_cmd_vel = self.create_publisher(
      Twist,
      '/cmd_vel',
      10)   

    # Declare velocities
    self.linear_velocity = 0.0
    self.angular_velocity = 0.15
      
  def execute_callback(self, goal_handle):
    """
    Action server callback to execute accepted goals.
    """  
    self.get_logger().info('Executing goal...')

    # Interim feedback
    feedback_msg = ConnectToChargingDock.Feedback()
    feedback_msg.current_battery_state = this_battery_state
    
    # While the battery is not charging
    while this_battery_state.power_supply_status != 1:
    
      # Publish the current battery state
      feedback_msg.current_battery_state = this_battery_state
      self.get_logger().info('NOT CHARGING...')
      goal_handle.publish_feedback(feedback_msg)
      
      # Send the velocity command to the robot by publishing to the topic
      cmd_vel_msg = Twist()
      cmd_vel_msg.linear.x = self.linear_velocity
      cmd_vel_msg.angular.z = self.angular_velocity
      self.publisher_cmd_vel.publish(cmd_vel_msg)
      
      time.sleep(0.1)
    
    # Stop the robot
    cmd_vel_msg = Twist()
    cmd_vel_msg.linear.x = 0.0
    cmd_vel_msg.angular.z = 0.0
    self.publisher_cmd_vel.publish(cmd_vel_msg)

    # Update feedback    
    feedback_msg.current_battery_state = this_battery_state
    goal_handle.publish_feedback(feedback_msg)
  
    # Indicate the goal was successful
    goal_handle.succeed()
    self.get_logger().info('CHARGING...')
    self.get_logger().info('Successfully connected to the charging dock!')
  
    # Create a result message of the action type
    result = ConnectToChargingDock.Result()
    
    # Update the final battery state
    result.final_battery_state = feedback_msg.current_battery_state
    
    return result

class BatteryStateSubscriber(Node):
    """
    Subscriber node to the current battery state
    """      
    def __init__(self):
  
      # Initialize the class using the constructor
      super().__init__('battery_state_subscriber')
    
      # Create a subscriber 
      # This node subscribes to messages of type
      # sensor_msgs/BatteryState
      self.subscription_battery_state = self.create_subscription(
        BatteryState,
        '/battery_status',
        self.get_battery_state,
        10)
      
    def get_battery_state(self, msg):
      """
      Update the current battery state.
      """
      global this_battery_state
      this_battery_state = msg
    
def main(args=None):
  """
  Entry point for the program.
  """

  # Initialize the rclpy library
  rclpy.init(args=args)
  
  try: 
  
    # Create the Action Server node
    connect_to_charging_dock_action_server = ConnectToChargingDockActionServer()
    
    # Create the Battery State subscriber node
    battery_state_subscriber = BatteryStateSubscriber()
    
    # Set up mulithreading
    executor = MultiThreadedExecutor(num_threads=4)
    executor.add_node(connect_to_charging_dock_action_server)
    executor.add_node(battery_state_subscriber)
    
    try:
      # Spin the nodes to execute the callbacks
      executor.spin()
    finally:
      # Shutdown the nodes
      executor.shutdown()
      connect_to_charging_dock_action_server.destroy_node()
      battery_state_subscriber.destroy_node()

  finally:
    # Shutdown
    rclpy.shutdown()

if __name__ == '__main__':
    main()

<exec_depend>geometry_msgs</exec_depend>
<exec_depend>sensor_msgs</exec_depend>
<exec_depend>std_msgs</exec_depend>

#!/usr/bin/env python3 
"""
Description:
  Action Client to connect to a battery charging dock.
  Action is successful when the power_supply_status goes from
  NOT_CHARGING(i.e. 3) to CHARGING(i.e. 1)
-------
Author: Addison Sears-Collins
Website: AutomaticAddison.com
Date: November 13, 2021
"""
# Import our action definition
from action_tutorials_interfaces.action import ConnectToChargingDock

# ROS Client Library for Python
import rclpy

# ActionClient library for ROS 2 Python
from rclpy.action import ActionClient

# Handles the creation of nodes
from rclpy.node import Node

# Handles BatteryState message
from sensor_msgs.msg import BatteryState

class ConnectToChargingDockActionClient(Node):
  """
  Create a ConnectToChargingDockActionClient class, 
  which is a subclass of the Node class.
  """
  def __init__(self):
  
    # Initialize the class using the constructor
    super().__init__('connect_to_charging_dock_action_client')
    
    # Instantiate a new action client
    # self, type of action, action name
    self._action_client = ActionClient(
      self,
      ConnectToChargingDock,
      'connect_to_charging_dock')
      
  def send_goal(self, desired_battery_state):
    """
    Action client to send the goal
    """  
    # Set the goal message
    goal_msg = ConnectToChargingDock.Goal()
    goal_msg.desired_battery_state = desired_battery_state    
   
    # Wait for the Action Server to launch
    self._action_client.wait_for_server()
    
    # Register a callback for when the future is complete
    self._send_goal_future = self._action_client.send_goal_async(goal_msg, feedback_callback=self.feedback_callback)    
    self._send_goal_future.add_done_callback(self.goal_response_callback) 
    
  def goal_response_callback(self, future): 
    """
    Get the goal_handle
    """
    goal_handle = future.result()
    if not goal_handle.accepted:
      self.get_logger().info('Goal rejected...')
      return
    
    self.get_logger().info('Goal accepted...')
    
    self._get_result_future = goal_handle.get_result_async()
    self._get_result_future.add_done_callback(self.get_result_callback)
  
  def get_result_callback(self, future):
    """
    Gets the result 
    """
    result = future.result().result
    self.get_logger().info('Result (1 = CHARGING, 3 = NOT CHARGING): {0}'.format(
      result.final_battery_state.power_supply_status))
    rclpy.shutdown()
    
  def feedback_callback(self, feedback_msg):
    feedback = feedback_msg.feedback
    self.get_logger().info(
      'Received feedback (1 = CHARGING, 3 = NOT CHARGING): {0}'.format(
      feedback.current_battery_state.power_supply_status))

def main(args=None):
  """
  Entry point for the program.
  """

  # Initialize the rclpy library
  rclpy.init(args=args)
  
  # Create the goal message
  desired_battery_state = BatteryState()
  desired_battery_state.voltage = 2.16
  desired_battery_state.percentage = 0.24
  desired_battery_state.power_supply_status = 1
  
  # Create the Action Client node
  action_client = ConnectToChargingDockActionClient()
  
  # Send the goal  
  action_client.send_goal(desired_battery_state)
  
  # Spin to execute callbacks
  rclpy.spin(action_client)
  
if __name__ == '__main__':
    main()