adding launch for bringup (#35)

* adding launch file and configuring setup.py accordingly

* handling parameters

* adding robot state publisher to launch

* fix launch and use loggers

* some more logging

elite_arm_driver/elite_arm_driver/elite_arm_move.py

@@ -40,7 +40,8 @@ def joint_move_cb(self, request, response):
         return response
 
     def stop_move_cb(self, request, response):
-        print("recv Stop Move")
+        self.get_logger().info("stop_move_server recieved command to stop the robot")
         result_ = self.elite_robot.stop()  # pylint: disable=E1101
         response.result = result_
+        # ToDo: Log the response
         return response 

elite_arm_driver/elite_arm_driver/elite_arm_trajectory_action.py

@@ -32,7 +32,7 @@ def goal_callback(self, goal_handle):
 
     def cancel_callback(self, goal_handle):
         # Accepts or rejects a client request to cancel an action
-        print("==========================stop===========================")
+        self.get_logger().info("Stopping initiated")
         self._stop_move()
         return CancelResponse.ACCEPT
 
@@ -79,11 +79,13 @@ def execute_callback(self, goal_handle):
 
         goal_handle.succeed()
 
-        # ToDo: Handle errors
+        # ToDo: Handle errors and failures
         result = FollowJointTrajectory.Result()
         result.error_code = result.SUCCESSFUL
         result.error_string = "Completed"
 
+        self.get_logger().info("FollowJointTrajectory Completed the Goal")
+
         return result
 
 

elite_arm_driver/elite_arm_driver/elite_driver_node.py

@@ -28,11 +28,17 @@ def __init__(self) -> None:
 
         self.elite_robot = None
 
-        # Setup ros parameters
-        self.ip_address = "10.1.30.80"
-        self.auto_connect = True
-        self.use_fake = False
-        timer_period = 0.01
+        # declare parameters
+        self.declare_parameter('ip_address', value = "10.1.30.80")
+        self.declare_parameter('auto_connect', value = True)
+        self.declare_parameter('use_fake', value = False)
+        self.declare_parameter('timer_period', value = 0.01)
+
+        # assign parameters
+        self.ip_address = self.get_parameter('ip_address').value
+        self.auto_connect = self.get_parameter('auto_connect').value
+        self.use_fake = self.get_parameter('use_fake').value
+        timer_period = self.get_parameter('timer_period').value
 
         # Seperate CB group for publishing joint states in parallel
         state_publisher_cb_grp = MutuallyExclusiveCallbackGroup()
@@ -50,10 +56,10 @@ def init_ec_sdk(self) -> None:
             self.elite_robot.robot_servo_on()
         self.elite_robot.monitor_thread_run()
         while self.elite_robot.monitor_info.machinePos[0] == None:
-            print("Monitor is not start,wait...")
+            self.get_logger().info('Monitor is not start, wait...', once=True)
             time.sleep(1)
 
-        print("Robot startup success...")
+        self.get_logger().info('Robot startup success...', once=True)
 
     def publish_states(self):
         EliteStatePublisher.update_states(self)

elite_arm_driver/elite_arm_driver/interpolate_five.py

@@ -18,7 +18,9 @@ def get_five_fun(time, start_joint, end_joint, start_speed=0, end_speed=0, start
           tf[1] + (3*start_acc-2*end_acc)*tf[2]) / (2*tf[4])
     a5 = (12*det - (6*end_speed+6*start_speed) *
           tf[1] - (start_acc-end_acc)*tf[2]) / (2*tf[5])
-    print(a0, a1, a2, a3, a4, a5)
+    # ToDo: Handle logging of the list of points of each trajectory
+
+    # print(a0, a1, a2, a3, a4, a5)
 
     def joint(t):
         tf = []

elite_arm_driver/launch/bringup.launch.py

@@ -0,0 +1,85 @@
+# Neobotix GmbH
+# Author: Pradheep Padmanabhan
+
+import launch
+from ament_index_python.packages import get_package_share_directory
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, OpaqueFunction
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import LaunchConfiguration, TextSubstitution, Command
+from launch_ros.actions import Node
+from launch.conditions import IfCondition
+import os
+from pathlib import Path
+from launch.launch_context import LaunchContext
+
+def execution_stage(context: LaunchContext):
+    ip_address = LaunchConfiguration('ip_address')
+    auto_connect = LaunchConfiguration('auto_connect')
+    use_fake = LaunchConfiguration('use_fake')
+    timer_period = LaunchConfiguration('timer_period')
+    start_rviz = LaunchConfiguration('start_rviz')
+
+    elite_description = os.path.join(get_package_share_directory('elite_description'), 'launch')
+
+    bringup_arm_driver = Node(
+        package="elite_arm_driver",
+        executable="elite",
+        parameters=[{
+            'ip_address': ip_address,
+            'auto_connect': auto_connect,
+            'use_fake': use_fake,
+            'timer_period': timer_period}]
+    )
+
+    rviz_launch = IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([elite_description, '/elite_description.launch.py']),
+            condition=IfCondition(start_rviz)
+        )
+
+    urdf = os.path.join(get_package_share_directory('elite_description'), 'urdf', 'ec66_description_real.urdf')
+
+    start_robot_state_publisher_cmd = Node(
+        package='robot_state_publisher',
+        executable='robot_state_publisher',
+        name='robot_state_publisher',
+        output='screen',
+        parameters=[{'robot_description':Command([
+            "xacro", " ", urdf])}]
+        )
+
+    return [
+        bringup_arm_driver,
+        rviz_launch,
+        start_robot_state_publisher_cmd]
+
+def generate_launch_description():
+    ip_addr_launch_arg = DeclareLaunchArgument(
+        'ip_address',
+        default_value='10.1.30.80')
+
+    auto_connect_launch_arg = DeclareLaunchArgument(
+        'auto_connect',
+        default_value='True')
+
+    use_fake_launch_arg = DeclareLaunchArgument(
+        'use_fake',
+        default_value='False')
+
+    timer_period_launch_arg = DeclareLaunchArgument(
+        'timer_period',
+        default_value=TextSubstitution(text='0.01'))
+
+    start_rviz_launch_arg = DeclareLaunchArgument(
+        'start_rviz',
+        default_value='False')
+
+    launch_args = []
+    launch_args.extend([ip_addr_launch_arg,
+        auto_connect_launch_arg,
+        use_fake_launch_arg,
+        timer_period_launch_arg,
+        start_rviz_launch_arg])
+
+    opq_function = OpaqueFunction(function=execution_stage)
+    return LaunchDescription(launch_args + [opq_function])

elite_arm_driver/setup.py

@@ -1,4 +1,6 @@
 from setuptools import setup
+from glob import glob
+import os
 
 package_name = 'elite_arm_driver'
 
@@ -10,13 +12,14 @@
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob(os.path.join('launch', '*.launch.py'))),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
     maintainer='Pradheep Padmanabhan',
     maintainer_email='[email protected]',
     description='TODO: Package description',
-    license='TODO: License declaration',
+    license='MIT',
     tests_require=['pytest'],
     entry_points={
         'console_scripts': ['elite = elite_arm_driver.elite_driver_node:main'