Skip to content

Commit

Permalink
Added home and cartesian pose action functionality
Browse files Browse the repository at this point in the history 
entry_point needed updates to handle multithreaded execution for reentrant callback groups
action servers in ROSRobot are also reentrant (parallel) that update in the step (also reentrant)
  • Loading branch information
@DasGuna
DasGuna committed Aug 2, 2023
1 parent 0ab2904 commit 660fb96
Show file tree
Hide file tree
Showing 2 changed files with 99 additions and 54 deletions.
31 changes: 21 additions & 10 deletions armer/entry_point.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,9 +12,11 @@
import os
import rclpy
import ament_index_python
from rclpy.node import Node
import numpy as np
from armer.armer import Armer
from std_msgs.msg import String
from rclpy.node import Node
from rclpy.executors import MultiThreadedExecutor
from rclpy.callback_groups import ReentrantCallbackGroup

__path__ = ament_index_python.packages.get_package_share_directory('armer')

Expand All @@ -39,12 +41,17 @@ def __init__(self):

# Setup the ROS timer callback to run the main ARMer functionality (step method)
self.last_time = self.get_clock().now()
self.timer = self.create_timer(1 / self.armer.frequency, self.timer_callback)
self.timer = self.create_timer(
timer_period_sec=(1 / self.armer.frequency),
callback=self.timer_callback,
callback_group=ReentrantCallbackGroup()
)

def timer_callback(self):
"""ROS2 Callback Mechanism
Set to configured frequency (init)
NOTE:
"""
current_time = self.get_clock().now()
# Get dt in seconds (REQUIRED)
Expand All @@ -61,15 +68,19 @@ def timer_callback(self):
def main(args=None):
rclpy.init(args=args)

armer = ArmerNode()
try:
armer = ArmerNode()
executor = MultiThreadedExecutor(num_threads=4)
executor.add_node(armer)

rclpy.spin(armer)
try:
executor.spin()
finally:
executor.shutdown()
armer.destroy_node()

# Destroy the node explicitly
# (optional - otherwise it will be done automatically
# when the garbage collector destroys the node object)
armer.destroy_node()
rclpy.shutdown()
finally:
rclpy.shutdown()

if __name__ == '__main__':
main()
122 changes: 78 additions & 44 deletions armer/robots/ROSRobot.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,7 @@
from armer.utils import mjtg, ikine
from armer.errors import ArmerError
from armer.trajectory import TrajectoryExecutor
from rclpy.callback_groups import ReentrantCallbackGroup

# pylint: disable=too-many-instance-attributes

Expand Down Expand Up @@ -52,13 +53,16 @@ def __init__(self,
frequency=None,
modified_qr=None,
singularity_thresh=0.02,
action_cb_group=None,
* args,
**kwargs): # pylint: disable=unused-argument

super().__init__(nh, *args, **kwargs)

# Setup the name of the robot
self.name = name if name else self.name
# print(f"name: {self.name}")
self.action_cb_group = action_cb_group if action_cb_group else ReentrantCallbackGroup()

# Handle the setup of joint stat and velocity topics for a ROS backend
# NOTE: this should match the ros_control real robot state and control (group velocity) interface topics
Expand Down Expand Up @@ -169,6 +173,7 @@ def __init__(self,
self.manip_scalar = None
self.singularity_approached = False

# Loads a configured named pose config
self.__load_named_pose_config()

#### --- ROS SETUP --- ###
Expand All @@ -181,11 +186,12 @@ def __init__(self,

# Setup interfaces for non-readonly
if not self.readonly:
# Publishers
# -- Publishers -- #
# NOTE: the joint publisher topic should attach to the ros_control input for real robot
self.joint_publisher = self.nh.create_publisher(
Float64MultiArray,
self.joint_velocity_topic,
1
Float64MultiArray,
self.joint_velocity_topic,
1
)
self.state_publisher = self.nh.create_publisher(
ManipulatorState,
Expand All @@ -197,17 +203,8 @@ def __init__(self,
'{}/cartesian/servo/arrived'.format(self.name.lower()),
1
)

# return

# # Services
# rospy.Service('{}/recover'.format(self.name.lower()),
# Empty, self.recover_cb)
# rospy.Service('{}/stop'.format(self.name.lower()),
# Empty, self.preempt)


# Subscribers

# -- Subscribers -- #
self.cartesian_velocity_subscriber = self.nh.create_subscription(
TwistStamped,
'{}/cartesian/velocity'.format(self.name.lower()),
Expand All @@ -227,13 +224,22 @@ def __init__(self,
10
)

# # Action Servers
# rclpy.action.ActionServer(
# self.nh,
# MoveToPose,
# '{}/cartesian/pose'.format(self.name.lower()),
# self.pose_cb
# )
# -- Action Servers -- #
self.move_pose_action = rclpy.action.ActionServer(
node=self.nh,
action_type=MoveToPose,
action_name='{}/cartesian/pose'.format(self.name.lower()),
execute_callback=self.cartesian_pose_cb,
callback_group=self.action_cb_group
)

self.home_action = rclpy.action.ActionServer(
node=self.nh,
action_type=Home,
action_name='{}/home'.format(self.name.lower()),
execute_callback=self.home_cb,
callback_group=self.action_cb_group
)
# self.velocity_server: actionlib.SimpleActionServer = actionlib.SimpleActionServer(
# '{}/cartesian/guarded_velocity'.format(self.name.lower()),
# GuardedVelocityAction,
Expand Down Expand Up @@ -270,14 +276,11 @@ def __init__(self,
# self.named_pose_server.register_preempt_callback(self.preempt)
# self.named_pose_server.start()

# self.home_server: actionlib.SimpleActionServer = actionlib.SimpleActionServer(
# '{}/home'.format(self.name.lower()),
# HomeAction,
# execute_cb=self.home_cb,
# auto_start=False
# )
# self.home_server.register_preempt_callback(self.preempt)
# self.home_server.start()
# # Services
# rospy.Service('{}/recover'.format(self.name.lower()),
# Empty, self.recover_cb)
# rospy.Service('{}/stop'.format(self.name.lower()),
# Empty, self.preempt)

# rospy.Service(
# '{}/set_cartesian_impedance'.format(self.name.lower()),
Expand Down Expand Up @@ -457,7 +460,7 @@ def servo_cb(self, msg: ServoStamped) -> None:
arrived_out.data = arrived
self.cartesian_servo_publisher.publish(arrived_out)

def pose_cb(self, goal):
def cartesian_pose_cb(self, goal_handle):
"""
ROS Action Server callback:
Moves the end-effector to the
Expand All @@ -466,35 +469,49 @@ def pose_cb(self, goal):
:param goal: [description]
:type goal: MoveToPoseGoal
"""
request = goal_handle._goal_request

# Check for movement and preempt
if self.moving:
self.preempt()

with self.lock:
goal_pose = goal.pose_stamped

# Take requested pose goal and resolve for execution
goal_pose = request.pose_stamped
if goal_pose.header.frame_id == '':
goal_pose.header.frame_id = self.base_link.name

goal_pose = self.transform(goal_pose, self.base_link.name)

pose = goal_pose.pose

# Calculate solution to pose and execute configured trajectory type
solution = ikine(self, pose, q0=self.q, end=self.gripper)

self.executor = TrajectoryExecutor(
self,
self.traj_generator(self, solution.q, goal.speed if goal.speed else 0.2)
self.traj_generator(self, solution.q, request.speed if request.speed else 0.2)
)

# Wait for finish (NOTE: this is a Reentrant Callback, so the step method updates)
while not self.executor.is_finished():
time.sleep(0.01)

# Set action server outputs
result = self.executor.is_succeeded()
if result:
goal_handle.succeed()
else:
goal_handle.abort()

self.executor = None
self.moving = False

return result
# Finalise result
action_result = MoveToPose.Result()
action_result.success = result

# DEBUGGING
# self.logger(f"action result: {action_result.success} | result: {result}")

return action_result

def joint_pose_cb(self, goal) -> None:
"""
Expand Down Expand Up @@ -558,34 +575,50 @@ def named_pose_cb(self, goal, result) -> None:

return result

def home_cb(self, goal) -> None:
"""[summary]
def home_cb(self, goal_handle) -> None:
"""ROS Action to Send Robot Home
:param req: Empty request
:type req: EmptyRequest
:return: Empty response
:rtype: EmptyResponse
"""
request = goal_handle._goal_request

# Check for movement and preempt
if self.moving:
self.preempt()

with self.lock:
# Get the configured ready state (joints) of the arm and execute configured trajectory type
qd = np.array(self.qr) if hasattr(self, 'qr') else self.q

self.executor = TrajectoryExecutor(
self,
self.traj_generator(self, qd, goal.speed if goal.speed else 0.2)
self.traj_generator(self, qd, request.speed if request.speed else 0.2)
)

# Wait for finish (NOTE: this is a Reentrant Callback, so the step method updates)
while not self.executor.is_finished():
time.sleep(0.01)
time.sleep(0.01)

# Set action server outputs
result = self.executor.is_succeeded()
if result:
goal_handle.succeed()
else:
goal_handle.abort()

self.executor = None
self.moving = False

return result
# Finalise result
action_result = Home.Result()
action_result.success = result

# DEBUGGING
# self.logger(f"action result: {action_result.success} | result: {result}")

return action_result

def recover_cb(self, req): # pylint: disable=no-self-use
"""[summary]
Expand Down Expand Up @@ -919,6 +952,7 @@ def step(self, dt: float = 0.01) -> None: # pylint: disable=unused-argument

## -- TRAJECTORY-BASED EXECUTION UPDATE -- ##
if self.executor:
# self.logger(f"Requested traj")
self.j_v = self.executor.step(dt)
else:
# Needed for preempting joint velocity control
Expand Down

0 comments on commit 660fb96

Please sign in to comment.