handling cancellations, goal conflicts and adding loggers (2) (#48)

* handling goal cancel

* reverting back the changes

* removing unwanted variable

* handling goals more elegantly

* more minors for goal handle

* clearing buffer at the sdk level for proper handling of new goal

* handle cancellations

* adding additional loggers

* some more loggers

elite_arm_driver/elite_arm_driver/elite_arm_kinematics.py

@@ -6,7 +6,7 @@
 
 class EliteArmKinematics():
     def __init__(self):
-        print("Listening for kinematics request")
+        self.get_logger().info("Listening for kinematics request")
         self.fk_srv = self.create_service(ForwardKinematic, 'forward_kinematics', self.forward_kinematics_cb)
         self.ik_srv = self.create_service(InverseKinematic, 'inverse_kinematics', self.inverse_kinematics_cb)
 
@@ -35,7 +35,7 @@ def inverse_kinematics_cb(self, request, response):
           target_point.append(math.degrees(angle))
 
         result = self.elite_robot.get_inverse_kinematic(
-            target_point, request.ref_joint,unit_type=0)
+            target_point, request.ref_joint, unit_type=0)
         if type(result) == tuple:
             response.result = False
         else:

elite_arm_driver/elite_arm_driver/elite_arm_move.py

@@ -4,7 +4,7 @@
 
 class EliteArmMove:
     def __init__(self, ):
-        print("ready to move arm")
+        self.get_logger().info("ready to move arm")
         self.cart_mov_srv = self.create_service(CartMove, 'cart_move_server', self.cart_move_cb)
         self.joint_mov_srv = self.create_service(JointMove, 'joint_move_server', self.joint_move_cb)
         self.stop_mov_srv = self.create_service(StopMove, 'stop_move_server', self.stop_move_cb)

elite_arm_driver/elite_arm_driver/elite_arm_set_io.py

@@ -4,7 +4,7 @@
 
 class EliteArmSetIO():
     def __init__(self):
-        print("starting elite arm set IO")
+        self.get_logger().info("starting elite arm set IO")
         self.digital_io_srv = self.create_service(SetIO, 'set_digital_io', self.digital_io_cb)
         self.analog_io_srv = self.create_service(SetAnalogIO, 'set_analog_io', self.analog_io_cb)
 

elite_arm_driver/elite_arm_driver/elite_arm_trajectory_action.py

@@ -13,29 +13,44 @@
 
 class EliteArmTrajectoryAction():
     def __init__(self, ):
-        print("starting elite arm trajectory action")
+        self.get_logger().info("starting elite arm trajectory action")
         self.action_server = ActionServer(
             self,
             FollowJointTrajectory,
             'arm_controller/follow_joint_trajectory',
             execute_callback=self.execute_callback,
-            callback_group=MutuallyExclusiveCallbackGroup(),
+            callback_group=ReentrantCallbackGroup(),
             goal_callback=self.goal_callback,
-            cancel_callback=self.cancel_callback)
+            cancel_callback=self.cancel_callback,
+            handle_accepted_callback=self.handle_accepted_callback)
         self.interpolate_sample_time = 0.008
         self.stop_mov_cli = self.create_client(StopMove, 'stop_move_server')
+        self.goal_handle_ = None
 
-    def goal_callback(self, goal_handle):
+    def goal_callback(self, goal_):
         self.get_logger().info('Goal request recieved')
-        self.goal = goal_handle
+        self.goal = goal_
         return GoalResponse.ACCEPT
 
     def cancel_callback(self, goal_handle):
         # Accepts or rejects a client request to cancel an action
         self.get_logger().info("Stopping initiated")
         self._stop_move()
+        self.goal_handle = None
         return CancelResponse.ACCEPT
 
+    def handle_accepted_callback(self, goal_handle):
+        # Takes care of multiple goal requests
+        if self.goal_handle_ != None:
+            if self.goal_handle_.is_active:
+                self.get_logger().info("Stopping the executing goal")
+                self._stop_move()
+                self.goal_handle_.canceled()
+
+        self.goal_handle_ = goal_handle
+        self.get_logger().info("Executing the new goal")
+        self.goal_handle_.execute()
+
     def execute_callback(self, goal_handle):
         points = self.goal.trajectory.points
         joints = []
@@ -59,26 +74,39 @@ def execute_callback(self, goal_handle):
             joint.append(0.0)
             joint.append(0.0)
 
-        is_blocking = True
-
         self.elite_robot.TT_init(t=time_stamp[1]*1000)
         last_joint = []
         for i in range(len(time_stamp)):
+            # check if cancellation is requested
+            if goal_handle.is_cancel_requested:
+                goal_handle.canceled()
+                self.get_logger().info("Returning stop from the execution")
+                result = FollowJointTrajectory.Result()
+                result.error_code = result.SUCCESSFUL
+                result.error_string = "Cancelled"
+
+                self.goal_handle_ = None
+                self.elite_robot.TT_clear_buff()
+                return result
+
             temp_joint = joint[i*8:i*8+6]
             self.elite_robot.TT_add_joint(temp_joint)
             last_joint = temp_joint
-        if is_blocking:
-            while 1:
-                time.sleep(0.1)
-                self.elite_robot:EC
-                current_joint = [round(i,1) for i in self.elite_robot.current_joint]
-                goal_joint = [round(i,1) for i in last_joint]
-                if (current_joint == goal_joint):
-                    self.elite_robot.TT_clear_buff()
-                    break
+
+        while 1:
+            time.sleep(0.1)
+            self.elite_robot:EC
+            current_joint = [round(i,1) for i in self.elite_robot.current_joint]
+            goal_joint = [round(i,1) for i in last_joint]
+            if (current_joint == goal_joint):
+                self.elite_robot.TT_clear_buff()
+                break
 
         goal_handle.succeed()
 
+        # reset the global goal_handle
+        self.goal_handle_ = None
+
         # ToDo: Handle errors and failures
         result = FollowJointTrajectory.Result()
         result.error_code = result.SUCCESSFUL
@@ -88,8 +116,6 @@ def execute_callback(self, goal_handle):
 
         return result
 
-
-
     def _stop_move(self) -> bool:
         request = StopMove.Request()
         self.future = self.stop_mov_cli.call_async(request)

elite_arm_driver/elite_arm_driver/elite_state_publisher.py

@@ -7,7 +7,7 @@
 
 class EliteStatePublisher():
     def __init__(self):
-        print("starting elite state publishers")
+        self.get_logger().info("starting elite state publishers")
         # Publish joint states
         self.joint_state_publisher = self.create_publisher(JointState, "joint_states", 10)
         # Todo Ros parameter