#!/usr/bin/env python3 

# Test code for controlling the lift mechanism on a warehouse robot
# Author: Addison Sears-Collins
# Website: https://automaticaddison.com

# ROS Client Library for Python
import rclpy
 
# Handles the creation of nodes
from rclpy.node import Node
 
# Enables usage of the standard message
import std_msgs.msg

# Enables use of sensor messages
import sensor_msgs.msg 

 
class LiftController(Node):
  """
  Create a LiftController class, which is a subclass of the Node class.
  """
  def __init__(self):
    """
    Class constructor to set up the node
    """
    # Initiate the Node class's constructor and give it a name
    super().__init__('lift_controller')
     
    # Create the publisher. This publisher will publish a JointState message
    # to a topic. The queue size is 10 messages.
    self.publisher_ = self.create_publisher(sensor_msgs.msg.JointState, 'joint_states', 10)

    # Create the subscriber. This subscriber will subscribe to a JointState message
    self.subscription = self.create_subscription(sensor_msgs.msg.JointState, 'joint_states', self.listener_callback, 10)
         
  def listener_callback(self, msg):
    """
    Callback function.
    """
    # Create a JointStates message
    new_msg = sensor_msgs.msg.JointState()
 
    # Set the message's data
    new_msg.header.stamp = self.get_clock().now().to_msg()
    new_msg.name = msg.name
    new_msg.position = msg.position
    new_msg.position[2] = 0.30 
     
    # Publish the message to the topic
    self.publisher_.publish(new_msg)
     
def main(args=None):
 
  # Initialize the rclpy library
  rclpy.init(args=args)
 
  # Create the node
  lift_controller = LiftController()
 
  # Spin the node so the callback function is called.
  rclpy.spin(lift_controller)
 
  # Destroy the node explicitly
  # (optional - otherwise it will be done automatically
  # when the garbage collector destroys the node object)
  lift_controller.destroy_node()
 
  # Shutdown the ROS client library for Python
  rclpy.shutdown()
 
if __name__ == '__main__':
  main()

#!/usr/bin/env python3

from two_wheeled_robot.module_to_import import ...

<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
  <name>two_wheeled_robot</name>
  <version>1.0.0</version>
  <description>A two-wheeled mobile robot with an IMU sensor and LIDAR</description>
  <maintainer email="automaticaddison@todo.todo">AutomaticAddison</maintainer>
  <license>MIT License</license>

  <buildtool_depend>ament_cmake</buildtool_depend>
  <buildtool_depend>ament_cmake_python</buildtool_depend>

  <exec_depend>rclcpp</exec_depend>
  <exec_depend>rclpy</exec_depend>
  <exec_depend>std_msgs</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>joint_state_publisher</exec_depend>
  <exec_depend>robot_state_publisher</exec_depend>
  <exec_depend>rviz</exec_depend>
  <exec_depend>xacro</exec_depend>
  <test_depend>ament_lint_auto</test_depend>
  <test_depend>ament_lint_common</test_depend>

  <export>
    <build_type>ament_cmake</build_type>
  </export>
</package>

cmake_minimum_required(VERSION 3.5)
project(two_wheeled_robot)

# Default to C99
if(NOT CMAKE_C_STANDARD)
  set(CMAKE_C_STANDARD 99)
endif()

# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
  set(CMAKE_CXX_STANDARD 14)
endif()

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# Find dependencies
find_package(ament_cmake REQUIRED)
find_package(ament_cmake_python REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclpy REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)

# Include cpp "include" directory
include_directories(include)

install(
  DIRECTORY config include launch maps media meshes models params rviz scripts src two_wheeled_robot urdf worlds
  DESTINATION share/${PROJECT_NAME}
)

# Install Python modules
ament_python_install_package(${PROJECT_NAME})

# Install Python executables
install(PROGRAMS
  scripts/lift_controller.py
  DESTINATION lib/${PROJECT_NAME}
)

if(BUILD_TESTING)
  find_package(ament_lint_auto REQUIRED)
  # the following line skips the linter which checks for copyrights
  # uncomment the line when a copyright and license is not present in all source files
  #set(ament_cmake_copyright_FOUND TRUE)
  # the following line skips cpplint (only works in a git repo)
  # uncomment the line when this package is not in a git repo
  #set(ament_cmake_cpplint_FOUND TRUE)
  ament_lint_auto_find_test_dependencies()
endif()

ament_package()

# Create Cpp executable
add_executable(cpp_executable src/cpp_node.cpp)
ament_target_dependencies(cpp_executable rclcpp)

# Install Cpp executables
install(TARGETS
  cpp_executable
  DESTINATION lib/${PROJECT_NAME}
)