Compare commits
14 Commits
master
...
library_v4
Author | SHA1 | Date | |
---|---|---|---|
d29be1215b | |||
efe333af73 | |||
01db96d38e | |||
cc15ef1caa | |||
e17c718d0b | |||
61ebd7ca68 | |||
95276b75da | |||
7d27ff3eba | |||
ae20df933b | |||
dc0e4a48cd | |||
f9973e9c78 | |||
216bc96396 | |||
6f9cdeb008 | |||
b0a370b65d |
21
README.md
21
README.md
@ -1,13 +1,16 @@
|
|||||||
# pubsub - Publisher & Subscriber Package
|
# pubsub - ROS2 Package
|
||||||
Minimal Publisher & Subscriber Template for ROS2.
|
Python Library for ROS2 Topic Publisher & Subscriber, Service Host & Client, Actions Server & Client.
|
||||||
|
|
||||||
|
Disclaimer:
|
||||||
```
|
```
|
||||||
Created for ROS Workshop 2020
|
Created for University Course "Roverentwicklung für Explorationsaufgaben"
|
||||||
Course Roverentwicklung für Explorationsaufgaben
|
|
||||||
Institute for Space Systems
|
Institute for Space Systems
|
||||||
University of Stuttgart.
|
University of Stuttgart.
|
||||||
|
|
||||||
|
Created by Patrick Winterhalder, Institute of Space Systems, University of Stuttgart (2020),
|
||||||
|
Edited by Christopher Herr (2023)
|
||||||
```
|
```
|
||||||
Class Code created by [Patrick Winterhalder](https://www.irs.uni-stuttgart.de/en/institute/team/Winterhalder/),
|
|
||||||
[IRS](https://www.irs.uni-stuttgart.de), University of Stuttgart.
|
|
||||||
|
|
||||||
|
|
||||||
## Workshop Prerequisites
|
## Workshop Prerequisites
|
||||||
@ -40,6 +43,12 @@ The following steps describe how you can create a local copy of this repository
|
|||||||
* Remember to run `source ~/<workspace_path>/install/local_setup.sh` after every build.
|
* Remember to run `source ~/<workspace_path>/install/local_setup.sh` after every build.
|
||||||
In order to avoid this repetitive command, add line 23-27 from [_.bashrc_](https://egit.irs.uni-stuttgart.de/RoverLehre/ROS2_pubsub/src/branch/master/non-ros-files/bashrc_overlay.bash) to your shell startup script for every overlay you want to source. This script runs everytime you start a new console, thus loading your overlays automatically. This is very handy if you are working with multiple _colcon_ workspaces (overlays) simultaneously.
|
In order to avoid this repetitive command, add line 23-27 from [_.bashrc_](https://egit.irs.uni-stuttgart.de/RoverLehre/ROS2_pubsub/src/branch/master/non-ros-files/bashrc_overlay.bash) to your shell startup script for every overlay you want to source. This script runs everytime you start a new console, thus loading your overlays automatically. This is very handy if you are working with multiple _colcon_ workspaces (overlays) simultaneously.
|
||||||
|
|
||||||
|
## Deprecation Error when building
|
||||||
|
Solution:
|
||||||
|
* https://answers.ros.org/question/386341/ros2-userwarning-usage-of-dash-separated-install-scripts-will-not-be-supported-in-future-versions-please-use-the-underscore-name-install_scripts/
|
||||||
|
* https://answers.ros.org/question/396439/setuptoolsdeprecationwarning-setuppy-install-is-deprecated-use-build-and-pip-and-other-standards-based-tools/
|
||||||
|
```$ pip install setuptools==58.2.0```
|
||||||
|
|
||||||
|
|
||||||
## Workshop Content
|
## Workshop Content
|
||||||
In this workshop you will:
|
In this workshop you will:
|
||||||
|
@ -36,13 +36,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
|
|||||||
"msg/CustomMsg1.msg"
|
"msg/CustomMsg1.msg"
|
||||||
"msg/CustomMsg2.msg"
|
"msg/CustomMsg2.msg"
|
||||||
"srv/CustomSrv1.srv"
|
"srv/CustomSrv1.srv"
|
||||||
"srv/StateMachineSrv.srv"
|
"action/TestACT.action"
|
||||||
"srv/InclineControlSRV.srv"
|
|
||||||
"srv/MosfetControlSRV.srv"
|
|
||||||
"srv/PitchControlSRV.srv"
|
|
||||||
"srv/RotationControlSRV.srv"
|
|
||||||
"srv/SensorSRV.srv"
|
|
||||||
"srv/WatchdogSRV.srv"
|
|
||||||
DEPENDENCIES builtin_interfaces
|
DEPENDENCIES builtin_interfaces
|
||||||
)
|
)
|
||||||
|
|
||||||
|
8
src/custom_interfaces/action/TestACT.action
Normal file
8
src/custom_interfaces/action/TestACT.action
Normal file
@ -0,0 +1,8 @@
|
|||||||
|
#Goal
|
||||||
|
int8 number
|
||||||
|
---
|
||||||
|
#Response/Result
|
||||||
|
bool success
|
||||||
|
---
|
||||||
|
#Feedback
|
||||||
|
int8 current_number
|
@ -13,10 +13,14 @@
|
|||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
<!-- ADD THESE LINES: START HERE -->
|
<!-- ADD THESE LINES: START HERE -->
|
||||||
|
|
||||||
<build_depend>builtin_interfaces</build_depend>
|
<build_depend>builtin_interfaces</build_depend>
|
||||||
<build_depend>rosidl_default_generators</build_depend>
|
<build_depend>rosidl_default_generators</build_depend>
|
||||||
|
<buildtool_depend>rosidl_default_generators</buildtool_depend>
|
||||||
|
<depend>action_msgs</depend>
|
||||||
|
|
||||||
|
|
||||||
<exec_depend>builtin_interfaces</exec_depend>
|
<exec_depend>builtin_interfaces</exec_depend>
|
||||||
<exec_depend>rosidl_default_runtime</exec_depend>
|
<exec_depend>rosidl_default_runtime</exec_depend>
|
||||||
|
58
src/pubsub/pubsub/action_client_example.py
Normal file
58
src/pubsub/pubsub/action_client_example.py
Normal file
@ -0,0 +1,58 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.action import ActionClient
|
||||||
|
from rclpy.node import Node
|
||||||
|
import time
|
||||||
|
|
||||||
|
|
||||||
|
from pubsub.pubsub_library_v4 import MinimalActionClient
|
||||||
|
|
||||||
|
from custom_interfaces.action import TestACT
|
||||||
|
|
||||||
|
|
||||||
|
class Client(MinimalActionClient):
|
||||||
|
|
||||||
|
def goalrequest(self):
|
||||||
|
# Create a goal request and send it
|
||||||
|
goal_msg = TestACT.Goal()
|
||||||
|
goal_msg.number = 7
|
||||||
|
Tester.send_action_goal(goal_msg)
|
||||||
|
|
||||||
|
# for only one variable:
|
||||||
|
# goal_msg = 7
|
||||||
|
# Tester.send_action_goal(goal_msg)
|
||||||
|
# is also valid
|
||||||
|
|
||||||
|
# Start a 5 second timer to initiate cancel request (demonstarate cancel)
|
||||||
|
self._timer = self.create_timer(5.0, self.cancel_goal)
|
||||||
|
|
||||||
|
|
||||||
|
def get_result_callback(self, future):
|
||||||
|
# get result
|
||||||
|
result = future.result().result
|
||||||
|
self.get_logger().info(f'Result: {result.success}')
|
||||||
|
|
||||||
|
|
||||||
|
def feedback_callback(self, feedback_msg):
|
||||||
|
# get feedback
|
||||||
|
feedback = feedback_msg.feedback
|
||||||
|
#print(feedback.current_number)
|
||||||
|
self.get_logger().info(f'Received feedback: {feedback.current_number}')
|
||||||
|
|
||||||
|
def spin_once(self):
|
||||||
|
rclpy.spin_once(self, timeout_sec=0.01)
|
||||||
|
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
global Tester
|
||||||
|
|
||||||
|
Tester = Client(NODE_NAME="action_client", ACT_NAME="ActionTest", ACT_TYPE=TestACT)
|
||||||
|
Tester.goalrequest()
|
||||||
|
|
||||||
|
|
||||||
|
while rclpy.ok():
|
||||||
|
Tester.spin_once()
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
68
src/pubsub/pubsub/action_server_example.py
Normal file
68
src/pubsub/pubsub/action_server_example.py
Normal file
@ -0,0 +1,68 @@
|
|||||||
|
import rclpy
|
||||||
|
from rclpy.action import ActionServer, CancelResponse, GoalResponse
|
||||||
|
from rclpy.node import Node
|
||||||
|
import time
|
||||||
|
|
||||||
|
from pubsub.pubsub_library_v4 import MinimalActionServer
|
||||||
|
|
||||||
|
from custom_interfaces.action import TestACT
|
||||||
|
|
||||||
|
|
||||||
|
class Server(MinimalActionServer):
|
||||||
|
|
||||||
|
def execute_callback(self, goal_handle):
|
||||||
|
self.get_logger().info('Executing goal...')
|
||||||
|
self.goal_handle = goal_handle
|
||||||
|
|
||||||
|
# Use the goal request
|
||||||
|
i = self.goal_handle.request.number
|
||||||
|
print(i)
|
||||||
|
time.sleep(2)
|
||||||
|
|
||||||
|
|
||||||
|
# initialize a feedback
|
||||||
|
self.feedback_msg = TestACT.Feedback()
|
||||||
|
|
||||||
|
|
||||||
|
# do the function with the requested input
|
||||||
|
while i < 100:
|
||||||
|
self.spin_once()
|
||||||
|
if self.cancel == True:
|
||||||
|
self.cancel = False
|
||||||
|
break
|
||||||
|
i += 1
|
||||||
|
|
||||||
|
# wrrte feedback
|
||||||
|
self.feedback_msg.current_number = i
|
||||||
|
self.get_logger().info(f'Feedback message: {self.feedback_msg.current_number}')
|
||||||
|
|
||||||
|
# publish feedback
|
||||||
|
Versuch.send_feedback(self.feedback_msg)
|
||||||
|
time.sleep(.5)
|
||||||
|
|
||||||
|
# genrate a respones/result
|
||||||
|
self.goal_handle.succeed()
|
||||||
|
self.result = TestACT.Result()
|
||||||
|
self.result.success = True
|
||||||
|
self.get_logger().info('Result sent.')
|
||||||
|
return self.result
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def spin_once(self):
|
||||||
|
rclpy.spin_once(self, timeout_sec=0.01)
|
||||||
|
|
||||||
|
|
||||||
|
def main(args=None):
|
||||||
|
rclpy.init(args=args)
|
||||||
|
global Versuch
|
||||||
|
|
||||||
|
Versuch = Server(NODE_NAME="action_server", ACT_NAME="ActionTest", ACT_TYPE=TestACT)
|
||||||
|
|
||||||
|
while rclpy.ok():
|
||||||
|
Versuch.spin_once()
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
if __name__ == '__main__':
|
||||||
|
main()
|
444
src/pubsub/pubsub/pubsub_library_v4.py
Normal file
444
src/pubsub/pubsub/pubsub_library_v4.py
Normal file
@ -0,0 +1,444 @@
|
|||||||
|
#!/usr/bin/env python
|
||||||
|
# -*- coding: utf-8 -*-
|
||||||
|
|
||||||
|
#********************************************#
|
||||||
|
# ROS2 Topic, Service and Action Class Library
|
||||||
|
|
||||||
|
# Creted for:
|
||||||
|
# ROS2 Workshop 2020
|
||||||
|
# Roverentwicklung für Explorationsaufgaben
|
||||||
|
# Institute for Space Systems
|
||||||
|
# University of Stuttgart
|
||||||
|
|
||||||
|
# Created by Patrick Winterhalder
|
||||||
|
# IRS, University of Stuttgart
|
||||||
|
|
||||||
|
# Updated by Christopher Herr
|
||||||
|
# Added Actions (March 2023)
|
||||||
|
#********************************************#
|
||||||
|
|
||||||
|
import rclpy
|
||||||
|
from rclpy.node import Node
|
||||||
|
from rclpy.action import ActionClient
|
||||||
|
from rclpy.action import ActionServer, CancelResponse
|
||||||
|
import time
|
||||||
|
|
||||||
|
# How to use, please refer to:
|
||||||
|
# Topic:
|
||||||
|
# - topic_talker.py
|
||||||
|
# - topic_listener.py
|
||||||
|
# Service:
|
||||||
|
# - service_host.py
|
||||||
|
# - service_client.py
|
||||||
|
# Action:
|
||||||
|
# - action_server_example.py
|
||||||
|
# - action_client_example.py
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
# Definition of Parent Classes
|
||||||
|
#******************************************************************************#
|
||||||
|
# TOPIC
|
||||||
|
#******************************************************************************#
|
||||||
|
|
||||||
|
class MinimalPublisher(Node):
|
||||||
|
|
||||||
|
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, MSG_PERIOD):
|
||||||
|
self.NODE_NAME= NODE_NAME
|
||||||
|
self.TOPIC_NAME = TOPIC_NAME
|
||||||
|
self.CUSTOM_MSG = MSG_TYPE
|
||||||
|
self.timer_period = MSG_PERIOD # [seconds]
|
||||||
|
# Init above laying class Node
|
||||||
|
super().__init__(self.NODE_NAME)
|
||||||
|
print("\t- " + str(TOPIC_NAME) + "\n")
|
||||||
|
self.publisher_ = self.create_publisher(
|
||||||
|
self.CUSTOM_MSG,
|
||||||
|
self.TOPIC_NAME,
|
||||||
|
10)
|
||||||
|
self.new_msg = False
|
||||||
|
# Define Node Frequency, equivalent to ~rospy.rate()
|
||||||
|
self.timer = self.create_timer(self.timer_period, self.publisher_timer)
|
||||||
|
return
|
||||||
|
|
||||||
|
def publisher_timer(self):
|
||||||
|
if self.new_msg is True:
|
||||||
|
try:
|
||||||
|
#self.get_logger().info('Pub:')
|
||||||
|
self.publisher_.publish(self.msg)
|
||||||
|
self.new_msg = False
|
||||||
|
except TypeError:
|
||||||
|
print("[ERROR] Msg-Data-Types do not match")
|
||||||
|
return
|
||||||
|
|
||||||
|
# Publish using Timer
|
||||||
|
def timer_publish(self, msg):
|
||||||
|
self.msg=msg
|
||||||
|
self.new_msg = True
|
||||||
|
return
|
||||||
|
|
||||||
|
# Publish directly without Timer
|
||||||
|
def direct_publish(self, msg):
|
||||||
|
try:
|
||||||
|
#self.get_logger().info('Pub:')
|
||||||
|
self.publisher_.publish(msg)
|
||||||
|
except TypeError:
|
||||||
|
print("[ERROR] Msg-Data-Types do not match")
|
||||||
|
return
|
||||||
|
|
||||||
|
#******************************************************************************#
|
||||||
|
|
||||||
|
class MinimalSubscriber(Node):
|
||||||
|
|
||||||
|
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS):
|
||||||
|
self.NODE_NAME= NODE_NAME
|
||||||
|
self.TOPIC_NAME = TOPIC_NAME
|
||||||
|
self.CUSTOM_MSG = MSG_TYPE
|
||||||
|
self.NUM_MSGS = NUM_MSGS
|
||||||
|
self.topic_received = False
|
||||||
|
self.overflow = False
|
||||||
|
self.watchdog = 0
|
||||||
|
self.trust = 0
|
||||||
|
self.msg = None
|
||||||
|
super().__init__(self.NODE_NAME)
|
||||||
|
self.subscription = self.create_subscription(
|
||||||
|
self.CUSTOM_MSG, # Message Type
|
||||||
|
self.TOPIC_NAME, # Topic Name
|
||||||
|
self.listener_callback, # Callback Function
|
||||||
|
self.NUM_MSGS) # List of saved messages
|
||||||
|
self.subscription # prevent unused variable warning
|
||||||
|
print("MinimalSubscriber `%s` created"%(self.NODE_NAME))
|
||||||
|
return
|
||||||
|
|
||||||
|
def listener_callback(self, msg):
|
||||||
|
#self.get_logger().info('I heard: "%s"' % msg.data)
|
||||||
|
self.last_msg_time = time.time()
|
||||||
|
self.topic_received = True
|
||||||
|
self.msg = msg
|
||||||
|
return
|
||||||
|
|
||||||
|
def has_msg(self):
|
||||||
|
# Check if a maessage has been received
|
||||||
|
return self.msg is not None
|
||||||
|
|
||||||
|
def return_msg(self):
|
||||||
|
return self.msg
|
||||||
|
|
||||||
|
def timer_overflow(self, time_limit=0, number_of_timer_overflows=10, trust_factor=1):
|
||||||
|
"""Watchdog for Topic Publisher Messages\n
|
||||||
|
ture = timer_overflow >> error
|
||||||
|
false = no timer_overflow >> no error"""
|
||||||
|
self.overflow = False
|
||||||
|
if time_limit > 0 and self.has_msg():
|
||||||
|
if time.time() <= self.last_msg_time + time_limit:
|
||||||
|
if self.watchdog > 0:
|
||||||
|
self.trust += 1
|
||||||
|
if self.trust > number_of_timer_overflows * trust_factor:
|
||||||
|
self.watchdog = 0
|
||||||
|
if time.time() > self.last_msg_time + time_limit:
|
||||||
|
self.watchdog += 1
|
||||||
|
self.trust = 0
|
||||||
|
self.get_logger().warn(f"Message-Watchdog triggered {self.watchdog} times")
|
||||||
|
if self.watchdog > number_of_timer_overflows:
|
||||||
|
self.overflow = True
|
||||||
|
return self.overflow
|
||||||
|
|
||||||
|
|
||||||
|
#******************************************************************************#
|
||||||
|
# SERVICE
|
||||||
|
#******************************************************************************#
|
||||||
|
|
||||||
|
class MinimalServiceProvider(Node):
|
||||||
|
""" Minimal Service Provider Class
|
||||||
|
Inputs:
|
||||||
|
* NODE_NAME: string
|
||||||
|
* SRV_NAME: string
|
||||||
|
* SRV_TYPE: service type class
|
||||||
|
* srv_callback_fct: service callback function
|
||||||
|
|
||||||
|
srv_callback_fct - Inputs:
|
||||||
|
* request: class of type srv request
|
||||||
|
* response: class of type srv response
|
||||||
|
|
||||||
|
Return:
|
||||||
|
* response: class of type srv response
|
||||||
|
"""
|
||||||
|
|
||||||
|
|
||||||
|
def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE, srv_callback_fct=None):
|
||||||
|
self.NODE_NAME = NODE_NAME
|
||||||
|
self.SRV_NAME = SRV_NAME
|
||||||
|
self.SRV_TYPE = SRV_TYPE
|
||||||
|
if srv_callback_fct == None:
|
||||||
|
self.srv_callback = self.std_srv_callback
|
||||||
|
else:
|
||||||
|
self.srv_callback = srv_callback_fct
|
||||||
|
# Init above laying class "Node"
|
||||||
|
super().__init__(self.NODE_NAME)
|
||||||
|
print("\tStarting Service as Host:\t%s"%(self.SRV_NAME))
|
||||||
|
#self.srv_host = self.create_service(self.SRV_TYPE, self.SRV_NAME, self.service_callback)
|
||||||
|
self.srv_host = self.create_service(self.SRV_TYPE, self.SRV_NAME, self.srv_callback)
|
||||||
|
|
||||||
|
#def service_callback(self, request, response):
|
||||||
|
def std_srv_callback(self, request, response):
|
||||||
|
"""
|
||||||
|
* request: first half of srv-file
|
||||||
|
* response: second half of srv-file
|
||||||
|
|
||||||
|
access values using class access:
|
||||||
|
* request.var_name
|
||||||
|
* response.var_name
|
||||||
|
"""
|
||||||
|
self.get_logger().info("[srv: %s] Service Call Received"%(self.SRV_NAME))
|
||||||
|
# Abstraction layer:
|
||||||
|
response = self.user_defined(request, response) # Function defined below
|
||||||
|
return response
|
||||||
|
|
||||||
|
# Insert your callback code here
|
||||||
|
def user_defined(self, request, response):
|
||||||
|
""" Write your user defined code here which will be run at every service call
|
||||||
|
|
||||||
|
Input:
|
||||||
|
* request: srv request class
|
||||||
|
Return:
|
||||||
|
* response: srv response class
|
||||||
|
|
||||||
|
Access these variables using class access, eg. request.var_name
|
||||||
|
"""
|
||||||
|
# Write user defined code here
|
||||||
|
# Adapt to fit your service type
|
||||||
|
# You could overwrite this fdunctions with a new function when initializing this class
|
||||||
|
response.success = True # Change this to fit your service type
|
||||||
|
return response
|
||||||
|
|
||||||
|
#******************************************************************************#
|
||||||
|
|
||||||
|
class MinimalServiceClientAsync(Node):
|
||||||
|
def __init__(self, NODE_NAME, SRV_NAME, SRV_TYPE):
|
||||||
|
self.NODE_NAME = NODE_NAME
|
||||||
|
self.SRV_NAME = SRV_NAME
|
||||||
|
self.SRV_TYPE = SRV_TYPE
|
||||||
|
# Init above laying class "Node" (super class)
|
||||||
|
super().__init__(self.NODE_NAME)
|
||||||
|
|
||||||
|
print("Starting Service Client:\t%s"%(self.SRV_NAME))
|
||||||
|
self.srv_client = self.create_client(self.SRV_TYPE, self.SRV_NAME)
|
||||||
|
while not self.srv_client.wait_for_service(timeout_sec=1.0):
|
||||||
|
self.get_logger().info('service not available, waiting...')
|
||||||
|
self.request = self.SRV_TYPE.Request()
|
||||||
|
self.done_was_true = False
|
||||||
|
|
||||||
|
def send_request(self, request):
|
||||||
|
"""
|
||||||
|
Feed request of type "SRV_TYPE.Request()"
|
||||||
|
Access variable using
|
||||||
|
* request = SRV_TYPE.Request()
|
||||||
|
* request.<var_name> = ....
|
||||||
|
"""
|
||||||
|
self.request = request
|
||||||
|
self.done_was_true = False
|
||||||
|
self.future = self.srv_client.call_async(self.request)
|
||||||
|
|
||||||
|
def check_if_service_complete(self):
|
||||||
|
""" Checks if service call was answered by host.
|
||||||
|
|
||||||
|
Returns tuple [done, response]:
|
||||||
|
* done: Bool (service call complete)
|
||||||
|
* response: msg class from srv type
|
||||||
|
"""
|
||||||
|
if self.future.done():
|
||||||
|
try:
|
||||||
|
response = self.future.result()
|
||||||
|
self.done_was_true = True
|
||||||
|
except Exception as e:
|
||||||
|
self.get_logger().info('Service call failed %r' % (e,))
|
||||||
|
else:
|
||||||
|
pass
|
||||||
|
# Adapt to fit your custom service type
|
||||||
|
# self.get_logger().info('Result of add_three_ints: for %d + %d + %d = %d' %(minimal_client.req.a, minimal_client.req.b, minimal_client.req.c, response.sum))
|
||||||
|
return self.future.done(), response # response is of type "<service_type_name>.Response()"
|
||||||
|
else:
|
||||||
|
return self.future.done(), None
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
#******************************************************************************#
|
||||||
|
# ACTIONS
|
||||||
|
#******************************************************************************#
|
||||||
|
|
||||||
|
class MinimalActionServer(Node):
|
||||||
|
|
||||||
|
def __init__(self, NODE_NAME, ACT_NAME, ACT_TYPE, execute_callback=None, cancel_callback=None):
|
||||||
|
"""MinimalActionCLien Input:\n
|
||||||
|
NODE_NAME: name of the Node
|
||||||
|
ACT_NAME: name of the action
|
||||||
|
ACT_TYPE: type of the action (custom action file)
|
||||||
|
execute_callback=None: user defined function to execute the action goal
|
||||||
|
cancel_callback=None: user defined function to cancel the goal during the action\n\n
|
||||||
|
Best practice is to inherit this class and then override the 'get_result_callback' and 'feedback_callback' method
|
||||||
|
in your own class with the same name."""
|
||||||
|
self.NODE_NAME = NODE_NAME
|
||||||
|
self.ACT_NAME = ACT_NAME
|
||||||
|
self.ACT_TYPE = ACT_TYPE
|
||||||
|
self.cancel = False
|
||||||
|
self.goal_handle = None
|
||||||
|
|
||||||
|
if execute_callback == None:
|
||||||
|
self.execute_callback = self.execute_callback
|
||||||
|
else:
|
||||||
|
self.execute_callback = execute_callback
|
||||||
|
|
||||||
|
if cancel_callback == None:
|
||||||
|
self.cancel_callback = self.cancel_callback
|
||||||
|
else:
|
||||||
|
self.cancel_callback = cancel_callback
|
||||||
|
|
||||||
|
super().__init__(self.NODE_NAME)
|
||||||
|
self._action_server = ActionServer(self, action_type=self.ACT_TYPE, action_name=self.ACT_NAME, execute_callback=self.execute_callback, cancel_callback=self.cancel_callback)
|
||||||
|
|
||||||
|
|
||||||
|
def execute_callback(self, goal_handle):
|
||||||
|
self.get_logger().info('Executing goal...')
|
||||||
|
self.goal_handle = goal_handle
|
||||||
|
|
||||||
|
time.sleep(1)
|
||||||
|
# initialize a feedback
|
||||||
|
self.get_logger().info('Sending feedback with vlaue: 34')
|
||||||
|
self.feedback_msg = self.ACT_TYPE.Feedback()
|
||||||
|
self.feedback_msg.current_number = 34
|
||||||
|
self.send_feedback(self.feedback_msg)
|
||||||
|
|
||||||
|
time.sleep(1)
|
||||||
|
# initzialize a result
|
||||||
|
self.get_logger().info('Sending result...')
|
||||||
|
result = self.ACT_TYPE.Result()
|
||||||
|
result.success = True
|
||||||
|
self.send_response_result()
|
||||||
|
return result
|
||||||
|
|
||||||
|
|
||||||
|
def send_feedback(self, feedback_msg):
|
||||||
|
"""Method to send feedback"""
|
||||||
|
if self.goal_handle is not None:
|
||||||
|
self.get_logger().info('Sending Feedback...')
|
||||||
|
self.feedback_msg = feedback_msg
|
||||||
|
self.goal_handle.publish_feedback(self.feedback_msg)
|
||||||
|
|
||||||
|
|
||||||
|
def cancel_callback(self, goal):
|
||||||
|
"""Accept or reject a client request to cancel an action."""
|
||||||
|
self.get_logger().info('Received cancel request')
|
||||||
|
self.cancel = True
|
||||||
|
return CancelResponse.ACCEPT
|
||||||
|
|
||||||
|
#******************************************************************************#
|
||||||
|
|
||||||
|
class MinimalActionClient(Node):
|
||||||
|
|
||||||
|
def __init__(self, NODE_NAME, ACT_NAME, ACT_TYPE, get_result_callback=None, feedback_callback=None):
|
||||||
|
"""MinimalActionCLien Input:\n
|
||||||
|
NODE_NAME: name of the Node
|
||||||
|
ACT_NAME: name of the action
|
||||||
|
ACT_TYPE: type of the action (custom action file)
|
||||||
|
get_result_callback=None: user defined function to receive the result of the action request
|
||||||
|
feedback_callback=None: user defined function to receive the feedback during the action\n\n
|
||||||
|
Best practice is to inherit this class and then override the 'get_result_callback' and 'feedback_callback' method
|
||||||
|
in your own class with the same name."""
|
||||||
|
self.NODE_NAME = NODE_NAME
|
||||||
|
self.ACT_NAME = ACT_NAME
|
||||||
|
self.ACT_TYPE = ACT_TYPE
|
||||||
|
self._goal_handle = None
|
||||||
|
|
||||||
|
if get_result_callback == None:
|
||||||
|
self.result_callback = self.get_result_callback
|
||||||
|
else:
|
||||||
|
self.result_callback = get_result_callback
|
||||||
|
|
||||||
|
if feedback_callback == None:
|
||||||
|
self.feedback = self.feedback_callback
|
||||||
|
else:
|
||||||
|
self.feedback = feedback_callback
|
||||||
|
|
||||||
|
super().__init__(self.NODE_NAME)
|
||||||
|
self._action_client = ActionClient(self, action_type=self.ACT_TYPE, action_name=self.ACT_NAME)
|
||||||
|
|
||||||
|
def send_action_goal(self, goal_msg=None):
|
||||||
|
"""
|
||||||
|
* request: first part of the action file
|
||||||
|
|
||||||
|
access values using class access:
|
||||||
|
* goal.var_name
|
||||||
|
"""
|
||||||
|
self.goal = self.ACT_TYPE.Goal()
|
||||||
|
|
||||||
|
self.goal_msg = goal_msg
|
||||||
|
#print(self.goal_msg.number)
|
||||||
|
|
||||||
|
# wait for server to be realdy
|
||||||
|
self._action_client.wait_for_server()
|
||||||
|
|
||||||
|
# send goal with feedback clallback
|
||||||
|
self._send_goal_future = self._action_client.send_goal_async(self.goal_msg, feedback_callback=self.feedback)
|
||||||
|
|
||||||
|
# get a response callback in the future
|
||||||
|
self._send_goal_future.add_done_callback(self.goal_response_callback)
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
def goal_response_callback(self, future):
|
||||||
|
self.goal_handle = future.result()
|
||||||
|
|
||||||
|
# chack if goal request was accepted
|
||||||
|
if not self.goal_handle.accepted:
|
||||||
|
self.get_logger().info('Goal rejected :(')
|
||||||
|
return
|
||||||
|
|
||||||
|
self._goal_handle = self.goal_handle
|
||||||
|
self.get_logger().info('Goal accepted :)')
|
||||||
|
|
||||||
|
# get result callback once request is done
|
||||||
|
self._get_result_future = self.goal_handle.get_result_async()
|
||||||
|
self._get_result_future.add_done_callback(self.result_callback)
|
||||||
|
|
||||||
|
|
||||||
|
def get_result_callback(self, future):
|
||||||
|
"""overwrite me
|
||||||
|
* result: second part of the action file"""
|
||||||
|
# get result
|
||||||
|
# -----------
|
||||||
|
# this is how to get the result:
|
||||||
|
#result = future.result().result
|
||||||
|
#sself.get_logger().info(f'Result: {result.success}')
|
||||||
|
|
||||||
|
self.get_logger().info('Get result function has been reached')
|
||||||
|
|
||||||
|
|
||||||
|
def feedback_callback(self, feedback_msg):
|
||||||
|
"""overwrite me"""
|
||||||
|
# get feedback
|
||||||
|
# -----------
|
||||||
|
# this is how to get the feedback:
|
||||||
|
#feedback = feedback_msg.feedback
|
||||||
|
#self.get_logger().info(f'Received feedback: {feedback.current_number}')
|
||||||
|
|
||||||
|
self.get_logger().info('Feedback function has been reached')
|
||||||
|
|
||||||
|
|
||||||
|
def cancel_done(self, future):
|
||||||
|
# get result of cancel request
|
||||||
|
cancel_response = future.result()
|
||||||
|
if len(cancel_response.goals_canceling) > 0:
|
||||||
|
self.get_logger().info('Goal successfully canceled')
|
||||||
|
else:
|
||||||
|
self.get_logger().info('Goal failed to cancel')
|
||||||
|
|
||||||
|
|
||||||
|
def cancel_goal(self):
|
||||||
|
"""Method to cancel the goal"""
|
||||||
|
if self._goal_handle is not None:
|
||||||
|
# send request to cancel goal
|
||||||
|
self.get_logger().info('Canceling goal')
|
||||||
|
# Cancel the goal
|
||||||
|
future = self._goal_handle.cancel_goal_async()
|
||||||
|
future.add_done_callback(self.cancel_done)
|
@ -5,7 +5,7 @@ from rclpy.node import Node
|
|||||||
from custom_interfaces.srv import CustomSrv1
|
from custom_interfaces.srv import CustomSrv1
|
||||||
|
|
||||||
# Service Minimal Host and Client
|
# Service Minimal Host and Client
|
||||||
from pubsub.pubsub_library_v3 import MinimalServiceClientAsync
|
from pubsub.pubsub_library_v4 import MinimalServiceClientAsync
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
def main(args=None):
|
||||||
|
@ -5,13 +5,13 @@ from rclpy.node import Node
|
|||||||
from custom_interfaces.srv import CustomSrv1
|
from custom_interfaces.srv import CustomSrv1
|
||||||
|
|
||||||
# Service Minimal Host and Client
|
# Service Minimal Host and Client
|
||||||
import pubsub.pubsub_library_v3 as lib
|
from pubsub.pubsub_library_v4 import MinimalServiceProvider
|
||||||
|
|
||||||
|
|
||||||
def main(args=None):
|
def main(args=None):
|
||||||
rclpy.init(args=args)
|
rclpy.init(args=args)
|
||||||
|
|
||||||
minimal_service = lib.MinimalServiceProvider(NODE_NAME="Srv_host_test_node", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1)
|
minimal_service = MinimalServiceProvider(NODE_NAME="Srv_host_test_node", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1)
|
||||||
|
|
||||||
while rclpy.ok():
|
while rclpy.ok():
|
||||||
try:
|
try:
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
import rclpy
|
import rclpy
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
|
||||||
import pubsub.pubsub_library_v3 as lib
|
from pubsub.pubsub_library_v4 import MinimalServiceClientAsync
|
||||||
|
|
||||||
# Service Type Files
|
# Service Type Files
|
||||||
from custom_interfaces.srv import StateMachineSrv
|
from custom_interfaces.srv import StateMachineSrv
|
||||||
@ -10,7 +10,7 @@ from custom_interfaces.srv import StateMachineSrv
|
|||||||
def main(args=None):
|
def main(args=None):
|
||||||
rclpy.init(args=args)
|
rclpy.init(args=args)
|
||||||
|
|
||||||
client_cmd_srv = lib.MinimalServiceClientAsync(NODE_NAME="client_cmd_srv_node", SRV_NAME="cmd", SRV_TYPE=StateMachineSrv)
|
client_cmd_srv = MinimalServiceClientAsync(NODE_NAME="client_cmd_srv_node", SRV_NAME="cmd", SRV_TYPE=StateMachineSrv)
|
||||||
|
|
||||||
# Create request
|
# Create request
|
||||||
srv_request = StateMachineSrv.Request()
|
srv_request = StateMachineSrv.Request()
|
||||||
|
@ -1,7 +1,7 @@
|
|||||||
import rclpy
|
import rclpy
|
||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
|
||||||
from pubsub.pubsub_library_v3 import MinimalServiceProvider
|
from pubsub.pubsub_library_v4 import MinimalServiceProvider
|
||||||
|
|
||||||
# Service Type Files
|
# Service Type Files
|
||||||
from custom_interfaces.srv import StateMachineSrv
|
from custom_interfaces.srv import StateMachineSrv
|
||||||
|
@ -18,7 +18,7 @@ import rclpy
|
|||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
|
||||||
# Import Subscriber Library
|
# Import Subscriber Library
|
||||||
from .pubsub_library_v3 import MinimalSubscriber
|
from pubsub.pubsub_library_v4 import MinimalSubscriber
|
||||||
|
|
||||||
# Import Message Types
|
# Import Message Types
|
||||||
from custom_interfaces.msg import CustomMsg1
|
from custom_interfaces.msg import CustomMsg1
|
||||||
|
@ -18,7 +18,7 @@ import rclpy
|
|||||||
from rclpy.node import Node
|
from rclpy.node import Node
|
||||||
|
|
||||||
# Import Publisher Library (Python Library)
|
# Import Publisher Library (Python Library)
|
||||||
from .pubsub_library import MinimalPublisher
|
from pubsub.pubsub_library_v4 import MinimalPublisher
|
||||||
|
|
||||||
# Import Message Types (Message Files)
|
# Import Message Types (Message Files)
|
||||||
from custom_interfaces.msg import CustomMsg1
|
from custom_interfaces.msg import CustomMsg1
|
||||||
|
@ -25,7 +25,10 @@ setup(
|
|||||||
'srvhost = pubsub.service_host:main',
|
'srvhost = pubsub.service_host:main',
|
||||||
'srvclient = pubsub.service_client:main',
|
'srvclient = pubsub.service_client:main',
|
||||||
'smhost = pubsub.statemachine_host_example:main',
|
'smhost = pubsub.statemachine_host_example:main',
|
||||||
'smclient = pubsub.statemachine_client_example:main'
|
'smclient = pubsub.statemachine_client_example:main',
|
||||||
|
'actclient = pubsub.action_client_example:main',
|
||||||
|
'actserver = pubsub.action_server_example:main',
|
||||||
|
'actsub = pubsub.sub:main',
|
||||||
],
|
],
|
||||||
},
|
},
|
||||||
)
|
)
|
||||||
|
Loading…
Reference in New Issue
Block a user