Add MoveIt 2 demo (#181)

* Add MoveIt2 configuration files

* Add launch file command

* Include moveit files

* Snapshot

* Fix urdf

* Interactive marker visible and action receives command

* Rewrite trajectory follower

* Add velocity trajectory

* Fix armed demo

* Run faster than real-time

* Add controller_name param

Author: lukicdarkoo

webots_ros2_core/webots_ros2_core/trajectory_follower.py

@@ -19,20 +19,20 @@
 import os
 import sys
 import argparse
-
 import rclpy
 from rclpy.time import Time
 from rclpy.node import Node
 from rosgraph_msgs.msg import Clock
-
 from webots_ros2_msgs.srv import SetInt
 from webots_ros2_core.joint_state_publisher import JointStatePublisher
 from webots_ros2_core.devices.device_manager import DeviceManager
 from webots_ros2_core.utils import get_node_name_from_args
-
 from webots_ros2_core.webots_controller import Supervisor
 
 
+MAX_REALTIME_FACTOR = 20
+
+
 class WebotsNode(Node):
     """
     Extends ROS2 base node to provide integration with Webots.
@@ -63,7 +63,7 @@ def __init__(self, name, args=None):
         self.timestep = int(self.robot.getBasicTimeStep())
         self.__clock_publisher = self.create_publisher(Clock, 'clock', 10)
         self.__step_service = self.create_service(SetInt, 'step', self.__step_callback)
-        self.__timer = self.create_timer(0.001 * self.timestep, self.__timer_callback)
+        self.__timer = self.create_timer((1 / MAX_REALTIME_FACTOR) * 1e-3 * self.timestep, self.__timer_callback)
         self.__device_manager = None
 
         # Joint state publisher
@@ -77,7 +77,7 @@ def start_joint_state_publisher(self):
 
     def step(self, ms):
         """Call this method on each step."""
-        if self.get_parameter('use_joint_state_publisher').value:
+        if self.__joint_state_publisher:
             self.__joint_state_publisher.publish()
         if self.__device_manager:
             self.__device_manager.step()

webots_ros2_core/webots_ros2_core/webots_node.py

@@ -29,11 +29,17 @@ class WebotsRoboticArmNode(WebotsNode):
         prefix (str): Prefix passed to JointStatePublisher.
     """
 
-    def __init__(self, name, args, prefix=''):
+    def __init__(self, name, args, prefix='', controller_name=''):
         super().__init__(name, args)
         self.start_joint_state_publisher()
         self.__prefix_param = self.declare_parameter('prefix', prefix)
-        self.__trajectory_follower = TrajectoryFollower(self.robot, self, joint_prefix=self.__prefix_param.value)
+        self.__controller_name_param = self.declare_parameter('controller_name', controller_name)
+        self.__trajectory_follower = TrajectoryFollower(
+            self.robot,
+            self,
+            joint_prefix=self.__prefix_param.value,
+            controller_name=self.__controller_name_param.value
+        )
 
         self.get_logger().info('Initializing robotic arm node with prefix = "%s"' % self.__prefix_param.value)
 

webots_ros2_core/webots_ros2_core/webots_robotic_arm_node.py

@@ -18,9 +18,9 @@
 
 import os
 import launch
+from ament_index_python.packages import get_package_share_directory
 from webots_ros2_core.utils import ControllerLauncher
 from webots_ros2_core.webots_launcher import WebotsLauncher
-from ament_index_python.packages import get_package_share_directory
 
 
 def generate_launch_description():
@@ -33,16 +33,14 @@ def generate_launch_description():
         package='webots_ros2_core',
         executable='webots_robotic_arm_node',
         arguments=['--webots-robot-name=abbirb4600'],
-        namespace='abb',
-        parameters=[{'synchronization': synchronization}],
+        parameters=[{'synchronization': synchronization, 'controller_name': 'abb'}],
         output='screen'
     )
     ure5_controller = ControllerLauncher(
         package='webots_ros2_core',
         executable='webots_robotic_arm_node',
         arguments=['--webots-robot-name=UR5e'],
-        namespace='ur',
-        parameters=[{'synchronization': synchronization}],
+        parameters=[{'synchronization': synchronization, 'controller_name': 'ur'}],
         output='screen'
     )
 

webots_ros2_demos/launch/armed_robots.launch.py

@@ -14,17 +14,23 @@
 
 """Trajectory follower client for the ABB irb4600 robot."""
 
-from webots_ros2_demos.follow_joint_trajectory_client import followJointTrajectoryClient
-
 import rclpy
+from webots_ros2_demos.follow_joint_trajectory_client import FollowJointTrajectoryClient
 
 
 def main(args=None):
     rclpy.init(args=args)
-    armedRobotUR = followJointTrajectoryClient('armed_robots_abb', '/abb/follow_joint_trajectory')
-    armedRobotUR.send_goal({
-        'joint_names': ['A motor', 'B motor', 'C motor', 'E motor', 'finger_1_joint_1',
-                        'finger_2_joint_1', 'finger_middle_joint_1'],
+    armed_robot_abb = FollowJointTrajectoryClient('armed_robots_abb', '/abb/follow_joint_trajectory')
+    armed_robot_abb.send_goal({
+        'joint_names': [
+            'A motor',
+            'B motor',
+            'C motor',
+            'E motor',
+            'finger_1_joint_1',
+            'finger_2_joint_1',
+            'finger_middle_joint_1'
+        ],
         'points': [
             {
                 'positions': [0.0, 0.0, 0.0, 0., 0.0, 0.0, 0.0],
@@ -82,7 +88,7 @@ def main(args=None):
             }
         ]
     }, 10)
-    rclpy.spin(armedRobotUR)
+    rclpy.spin(armed_robot_abb)
 
 
 if __name__ == '__main__':

webots_ros2_demos/webots_ros2_demos/armed_robots_abb.py

@@ -14,18 +14,23 @@
 
 """Trajectory follower client for the UR5 robot."""
 
-from webots_ros2_demos.follow_joint_trajectory_client import followJointTrajectoryClient
-
 import rclpy
+from webots_ros2_demos.follow_joint_trajectory_client import FollowJointTrajectoryClient
 
 
 def main(args=None):
     rclpy.init(args=args)
-    armedRobotUR = followJointTrajectoryClient('armed_robots_ur', '/ur/follow_joint_trajectory')
-    armedRobotUR.send_goal({
-        'joint_names': ['shoulder_pan_joint', 'shoulder_lift_joint', 'elbow_joint',
-                        'wrist_1_joint', 'finger_1_joint_1', 'finger_2_joint_1',
-                        'finger_middle_joint_1'],
+    armed_robot_ur = FollowJointTrajectoryClient('armed_robots_ur', '/ur/follow_joint_trajectory')
+    armed_robot_ur.send_goal({
+        'joint_names': [
+            'shoulder_pan_joint',
+            'shoulder_lift_joint',
+            'elbow_joint',
+            'wrist_1_joint',
+            'finger_1_joint_1',
+            'finger_2_joint_1',
+            'finger_middle_joint_1'
+        ],
         'points': [
             {
                 'positions': [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
@@ -71,7 +76,7 @@ def main(args=None):
             }
         ]
     }, 10)
-    rclpy.spin(armedRobotUR)
+    rclpy.spin(armed_robot_ur)
 
 
 if __name__ == '__main__':

webots_ros2_demos/webots_ros2_demos/armed_robots_ur.py

@@ -24,64 +24,67 @@
 from rclpy.node import Node
 
 
-class followJointTrajectoryClient(Node):
+class FollowJointTrajectoryClient(Node):
 
     def __init__(self, name, actionName):
         super().__init__(name)
-        self.client = ActionClient(self, FollowJointTrajectory, actionName)
-        self.remainingIteration = 0
-        self.currentTrajectory = None
+        self.__client = ActionClient(self, FollowJointTrajectory, actionName)
+        self.__remaining_iteration = 0
+        self.__current_trajectory = None
+        self.__get_result_future = None
+        self.__send_goal_future = None
 
-    def goal_response_callback(self, future):
+    def __on_goal_response_callback(self, future):
         goal_handle = future.result()
         if not goal_handle.accepted:
             self.get_logger().info('Goal rejected by action server.')
             return
 
         self.get_logger().info('Goal accepted by action server.')
+        self.__get_result_future = goal_handle.get_result_async()
+        self.__get_result_future.add_done_callback(self.__on_get_result_callback)
 
-        self._get_result_future = goal_handle.get_result_async()
-        self._get_result_future.add_done_callback(self.get_result_callback)
-
-    def feedback_callback(self, feedback):
+    def __on_feedback_callback(self, _):
         self.get_logger().info('Received feedback from action server.')
 
-    def get_result_callback(self, future):
+    def __on_get_result_callback(self, future):
         status = future.result().status
         if status == GoalStatus.STATUS_SUCCEEDED:
             self.get_logger().info('Goal succeeded.')
         else:
             self.get_logger().info('Goal failed with status: {0}'.format(status))
 
-        if self.remainingIteration > 0:
-            self.send_goal(self.currentTrajectory, self.remainingIteration - 1)
+        if self.__remaining_iteration > 0:
+            self.send_goal(self.__current_trajectory, self.__remaining_iteration - 1)
         else:
             # Shutdown after receiving a result
             rclpy.shutdown()
 
     def send_goal(self, trajectory, iteration=1):
         self.get_logger().info('Waiting for action server...')
-        self.client.wait_for_server()
+        self.__client.wait_for_server()
 
-        self.currentTrajectory = trajectory
-        self.remainingIteration = iteration - 1
+        self.__current_trajectory = trajectory
+        self.__remaining_iteration = iteration - 1
 
         goal_msg = FollowJointTrajectory.Goal()
         goal_msg.trajectory.joint_names = trajectory['joint_names']
         for point in trajectory['points']:
-            trajectoryPoint = JointTrajectoryPoint(positions=point['positions'],
-                                                   velocities=point['velocities'],
-                                                   accelerations=point['accelerations'],
-                                                   time_from_start=Duration(
-                                                       sec=point['time_from_start']['sec'],
-                                                       nanosec=point['time_from_start']['nanosec']
-                                                    ))
-            goal_msg.trajectory.points.append(trajectoryPoint)
+            trajectory_point = JointTrajectoryPoint(
+                positions=point['positions'],
+                velocities=point['velocities'],
+                accelerations=point['accelerations'],
+                time_from_start=Duration(
+                    sec=point['time_from_start']['sec'],
+                    nanosec=point['time_from_start']['nanosec']
+                )
+            )
+            goal_msg.trajectory.points.append(trajectory_point)
 
         self.get_logger().info('Sending goal request...')
 
-        self._send_goal_future = self.client.send_goal_async(
+        self.__send_goal_future = self.__client.send_goal_async(
             goal_msg,
-            feedback_callback=self.feedback_callback)
-
-        self._send_goal_future.add_done_callback(self.goal_response_callback)
+            feedback_callback=self.__on_feedback_callback
+        )
+        self.__send_goal_future.add_done_callback(self.__on_goal_response_callback)

webots_ros2_demos/webots_ros2_demos/follow_joint_trajectory_client.py

@@ -0,0 +1,14 @@
+controller_names:
+  - webots_controller
+
+webots_controller:
+  action_ns: follow_joint_trajectory
+  type: FollowJointTrajectory
+  default: true
+  joints:
+    - shoulder_pan_joint
+    - shoulder_lift_joint
+    - elbow_joint
+    - wrist_1_joint
+    - wrist_2_joint
+    - wrist_3_joint

webots_ros2_universal_robot/config/controllers.yaml

@@ -0,0 +1,4 @@
+manipulator:
+  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+  kinematics_solver_search_resolution: 0.005
+  kinematics_solver_timeout: 0.005