Add Launch support

README.md

@@ -87,7 +87,7 @@ ROS2 Solution for FANUC robots
    ```
 3. Clone repo
    ```sh
-   https://github.com/UofI-CDACS/fanuc_ros2_drivers.git
+   git clone git clone https://github.com/UofI-CDACS/fanuc_ros2_drivers/ --branch launch-dev
    ```
 4. Resolve Dependencies
    ```sh
@@ -97,10 +97,7 @@ ROS2 Solution for FANUC robots
    ```sh
    colcon build
    ```
-6. Turn script into an executable
-   ```sh
-   chmod +x startRobot.sh
-   ```
+
    _Full guide here: [ROS2 Humble Documentation](https://docs.ros.org/en/humble/Tutorials/Beginner-Client-Libraries/Creating-A-Workspace/Creating-A-Workspace.html)_
 
    Installation Complete
@@ -112,18 +109,22 @@ ROS2 Solution for FANUC robots
 
 ## Running Nodes
 1. Open new terminal and go to workspace
-2. Run script
+2. Source overlay
    ```sh
-   ./startRobot.sh XXX.XXX.XXX.XXX ROBOT_NAME # Fill X's with Robot IP Address and name
+   source install/setup.sh
    ```
-	! Note: This script should only be run once per robot! Running multiple times can cause your computer to crash.
-  - This script only works for one robot. If you need to start more than 1 robot, this should be done in mulitple terminals.
-3. Open new terminal and go to workspace
-4. Source overlay
+3. Run launch
+   ```sh
+   ros2 launch launch/start.launch.py robot_name:=NAME_OF_ROBOT robot_ip:=0.0.0.0 # Parameters must be formated this way or the command will give you an error
+   ```
+  - This script only works for one robot. If you need to start more than 1 robot, this should be done in mulitple terminals. (WIP)
+## Using Nodes
+1. Open new terminal and go to workspace
+2. Source overlay
    ```sh
    source install/setup.sh
    ```
-5. Start using!
+3. Start using!
 
 
 _For more examples, please refer to the [Documentation](https://example.com)_

launch/start.launch.py

@@ -0,0 +1,60 @@
+from launch_ros.substitutions import FindPackageShare
+
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, TextSubstitution
+import sys
+
+name = ''
+ip = ''
+
+# It has to be passed like this, otherwise launch gets upset
+for arg in sys.argv:
+    if arg.startswith("robot_name:="):
+        name = arg.split(":=")[1]
+    elif arg.startswith("robot_ip:="):
+    	ip = arg.split(":=")[1]
+
+def generate_launch_description():
+    return LaunchDescription([
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                PathJoinSubstitution([
+                    FindPackageShare('action_servers'),
+                    'launch',
+                    'action_servers.launch.py'
+                ])
+            ]),
+            launch_arguments={
+                'robot_name': name,
+                'robot_ip': ip,
+            }.items()
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                PathJoinSubstitution([
+                    FindPackageShare('msg_publishers'),
+                    'launch',
+                    'message_publishers.launch.py'
+                ])
+            ]),
+            launch_arguments={
+                'robot_name': name,
+                'robot_ip': ip,
+            }.items()
+        ),
+        IncludeLaunchDescription(
+            PythonLaunchDescriptionSource([
+                PathJoinSubstitution([
+                    FindPackageShare('srv_services'),
+                    'launch',
+                    'srv_services.launch.py'
+                ])
+            ]),
+            launch_arguments={
+                'robot_name': name,
+                'robot_ip': ip,
+            }.items()
+        ),
+    ])

src/action_servers/action_servers/cart_pose_server.py

@@ -3,10 +3,9 @@
 import os
 import rclpy
 
-sys.path.append("src/dependencies/")
-import FANUCethernetipDriver
+import dependencies.FANUCethernetipDriver as FANUCethernetipDriver
 
-from robot_controller import robot
+from dependencies.robot_controller import robot
 from fanuc_interfaces.action import CartPose
 from rclpy.node import Node
 from rclpy.action import ActionServer, GoalResponse, CancelResponse
@@ -15,26 +14,21 @@
 
 sys.path.append('./pycomm3/pycomm3')
 
-# Robot IP is passed as command line argument 1
-robot_ip = sys.argv[1]
-name = sys.argv[2]
-
-# # Quick and dirty
-# if robot_ip == '172.29.208.124':
-# 	name = "beaker"
-# elif robot_ip == '172.29.208.123':
-#      name = "bunsen"
-# else:
-# 	name = "rogue"
 
 class cart_pose_server(Node):
     def __init__(self):
         super().__init__('cart_pose_server')
 
+        self.declare_parameters(
+            namespace='',
+            parameters=[('robot_ip','172.29.208.0'),
+                        ('robot_name','noNAME')] # custom, default
+        )
+
         self.goal = CartPose.Goal()
-        self.bot = robot(robot_ip)
+        self.bot = robot(self.get_parameter('robot_ip').value)
 
-        self._action_server = ActionServer(self, CartPose, f'{name}/cartesian_pose', 
+        self._action_server = ActionServer(self, CartPose, f"/{self.get_parameter('robot_name').value}/cartesian_pose", 
                                         execute_callback = self.execute_callback, 
                                         goal_callback = self.goal_callback,
                                         cancel_callback = self.cancel_callback)
@@ -133,4 +127,3 @@ def main(args=None):
 
 if __name__ == '__main__':
     main()
-

src/action_servers/action_servers/convey_server.py

@@ -3,10 +3,9 @@
 import os
 import rclpy
 
-sys.path.append("src/dependencies/")
-import FANUCethernetipDriver
+import dependencies.FANUCethernetipDriver as FANUCethernetipDriver
 
-from robot_controller import robot
+from dependencies.robot_controller import robot
 from fanuc_interfaces.action import Conveyor
 from rclpy.node import Node
 from rclpy.action import ActionServer, GoalResponse, CancelResponse
@@ -15,26 +14,21 @@
 
 sys.path.append('./pycomm3/pycomm3')
 
-# Robot IP is passed as command line argument 1
-robot_ip = sys.argv[1]
-name = sys.argv[2]
-
-# # Quick and dirty
-# if robot_ip == '172.29.208.124':
-# 	name = "beaker"
-# elif robot_ip == '172.29.208.123':
-#      name = "bunsen"
-# else:
-# 	name = "rogue"
 
 class convey_server(Node):
     def __init__(self):
         super().__init__('convey_server')
 
+        self.declare_parameters(
+            namespace='',
+            parameters=[('robot_ip','172.29.208.0'),
+                        ('robot_name','noNAME')] # custom, default
+        )
+
         self.goal = Conveyor.Goal()
-        self.bot = robot(robot_ip)
+        self.bot = robot(self.get_parameter('robot_ip').value)
 
-        self._action_server = ActionServer(self, Conveyor, f'{name}/conveyor', 
+        self._action_server = ActionServer(self, Conveyor, f"/{self.get_parameter('robot_name').value}/conveyor", 
                                         execute_callback = self.execute_callback, 
                                         goal_callback = self.goal_callback,
                                         cancel_callback = self.cancel_callback)

src/action_servers/action_servers/joint_pose_server.py

@@ -3,10 +3,9 @@
 import os
 import rclpy
 
-sys.path.append("src/dependencies/")
-import FANUCethernetipDriver
+import dependencies.FANUCethernetipDriver as FANUCethernetipDriver
 
-from robot_controller import robot
+from dependencies.robot_controller import robot
 from fanuc_interfaces.action import JointPose
 from rclpy.node import Node
 from rclpy.action import ActionServer, GoalResponse, CancelResponse
@@ -15,26 +14,21 @@
 
 sys.path.append('./pycomm3/pycomm3')
 
-# Robot IP is passed as command line argument 1
-robot_ip = sys.argv[1]
-name = sys.argv[2]
-
-# # Quick and dirty
-# if robot_ip == '172.29.208.124':
-# 	name = "beaker"
-# elif robot_ip == '172.29.208.123':
-#      name = "bunsen"
-# else:
-# 	name = "rogue"
 
 class joint_pose_server(Node):
     def __init__(self):
         super().__init__('joint_pose_server')
 
+        self.declare_parameters(
+            namespace='',
+            parameters=[('robot_ip','172.29.208.0'),
+                        ('robot_name','noNAME')] # custom, default
+        )
+
         self.goal = JointPose.Goal()
-        self.bot = robot(robot_ip)
+        self.bot = robot(self.get_parameter('robot_ip').value)
 
-        self._action_server = ActionServer(self, JointPose, f'{name}/joint_pose', 
+        self._action_server = ActionServer(self, JointPose, f"{self.get_parameter('robot_name').value}/joint_pose", 
                                         execute_callback = self.execute_callback, 
                                         goal_callback = self.goal_callback,
                                         cancel_callback = self.cancel_callback)

src/action_servers/action_servers/onrobot_server.py

@@ -3,10 +3,9 @@
 import os
 import rclpy
 
-sys.path.append("src/dependencies/")
-import FANUCethernetipDriver
+import dependencies.FANUCethernetipDriver as FANUCethernetipDriver
 
-from robot_controller import robot
+from dependencies.robot_controller import robot
 from fanuc_interfaces.action import OnRobotGripper
 from rclpy.node import Node
 from rclpy.action import ActionServer, GoalResponse, CancelResponse
@@ -15,26 +14,21 @@
 
 sys.path.append('./pycomm3/pycomm3')
 
-# Robot IP is passed as command line argument 1
-robot_ip = sys.argv[1]
-name = sys.argv[2]
-
-# # Quick and dirty
-# if robot_ip == '172.29.208.124':
-# 	name = "beaker"
-# elif robot_ip == '172.29.208.123':
-#      name = "bunsen"
-# else:
-# 	name = "rogue"
 
 class onrobot_gripper_server(Node):
     def __init__(self):
         super().__init__('onrobot_gripper_server')
 
+        self.declare_parameters(
+            namespace='',
+            parameters=[('robot_ip','172.29.208.0'),
+                        ('robot_name','noNAME')] # custom, default
+        )
+
         self.goal = OnRobotGripper.Goal()
-        self.bot = robot(robot_ip)
+        self.bot = robot(self.get_parameter('robot_ip').value)
 
-        self._action_server = ActionServer(self, OnRobotGripper, f'{name}/onrobot_gripper', 
+        self._action_server = ActionServer(self, OnRobotGripper, f"{self.get_parameter('robot_name').value}/onrobot_gripper", 
                                         execute_callback = self.execute_callback, 
                                         goal_callback = self.goal_callback,
                                         cancel_callback = self.cancel_callback)
@@ -66,7 +60,7 @@ def cancel_callback(self, goal_handle):
 
     async def execute_callback(self, goal_handle):
         # WIP: Add Try/Except to catch possible error
-        self.bot.onrobot_gripper(self.goal.width, self.goal.force)
+        self.bot.onRobot_gripper(self.goal.width, self.goal.force)
 
         goal_handle.succeed()
         result = OnRobotGripper.Result()

src/action_servers/action_servers/schunk_server.py

@@ -3,10 +3,10 @@
 import os
 import rclpy
 
-sys.path.append("src/dependencies/")
-import FANUCethernetipDriver
 
-from robot_controller import robot
+import dependencies.FANUCethernetipDriver as FANUCethernetipDriver
+
+from dependencies.robot_controller import robot
 from fanuc_interfaces.action import SchunkGripper
 from rclpy.node import Node
 from rclpy.action import ActionServer, GoalResponse, CancelResponse
@@ -15,26 +15,21 @@
 
 sys.path.append('./pycomm3/pycomm3')
 
-# Robot IP is passed as command line argument 1
-robot_ip = sys.argv[1]
-name = sys.argv[2]
-
-# # Quick and dirty
-# if robot_ip == '172.29.208.124':
-# 	name = "beaker"
-# elif robot_ip == '172.29.208.123':
-#      name = "bunsen"
-# else:
-# 	name = "rogue"
 
 class schunk_gripper_server(Node):
     def __init__(self):
         super().__init__('schunk_gripper_server')
 
+        self.declare_parameters(
+            namespace='',
+            parameters=[('robot_ip','172.29.208.0'),
+                        ('robot_name','noNAME')] # custom, default
+        )
+
         self.goal = SchunkGripper.Goal()
-        self.bot = robot(robot_ip)
+        self.bot = robot(self.get_parameter('robot_ip').value)
 
-        self._action_server = ActionServer(self, SchunkGripper, f'{name}/schunk_gripper', 
+        self._action_server = ActionServer(self, SchunkGripper, f"{self.get_parameter('robot_name').value}/schunk_gripper", 
                                         execute_callback = self.execute_callback, 
                                         goal_callback = self.goal_callback,
                                         cancel_callback = self.cancel_callback)