Compare commits

...

14 Commits

Author SHA1 Message Date
winterhalderp d29be1215b Update README.md 2024-04-24 16:10:03 +02:00
winterhalderp efe333af73 Update README.md 2024-04-24 14:22:25 +02:00
winterhalderp 01db96d38e Update README.md 2024-04-24 13:50:59 +02:00
winterhalderp cc15ef1caa Update README.md 2023-08-11 23:55:49 +02:00
Christopher Herr e17c718d0b Update 'src/pubsub/pubsub/pubsub_library_v4.py'
Changed the order so that the time is not overflowing while checking
2023-05-19 17:57:46 +02:00
Christopher Herr 61ebd7ca68 Update 'src/pubsub/pubsub/pubsub_library_v4.py'
Added has_msg request to only start counting once the publishing has started
2023-05-08 20:27:54 +02:00
Christopher Herr 95276b75da Update 'src/pubsub/pubsub/pubsub_library_v4.py'
Added trust to reset the watchdog
2023-05-08 20:17:14 +02:00
Christopher Herr 7d27ff3eba Update 'src/pubsub/pubsub/pubsub_library_v4.py'
Added the possibility to input a certain amount of allowed overflows until watchdog is triggered and reset overflow to false every time its called to reevaluate.
2023-05-08 19:50:06 +02:00
Christopher Herr ae20df933b Update 'src/pubsub/pubsub/pubsub_library_v4.py' 2023-04-20 19:58:18 +02:00
Christopher Herr dc0e4a48cd Update 'src/pubsub/pubsub/pubsub_library_v4.py' 2023-04-20 19:51:34 +02:00
Christopher Herr f9973e9c78 Changed the Imports 2023-03-25 16:33:56 +01:00
Christopher Herr 216bc96396 Implemented the Message Overfolw Watchdog 2023-03-25 16:23:01 +01:00
Christopher Herr 6f9cdeb008 Update pubsub to version 4, implementing Actions 2023-03-25 16:01:46 +01:00
Christopher Herr b0a370b65d first commit to new branch 2023-03-25 15:45:55 +01:00
14 changed files with 610 additions and 22 deletions

View File

@ -1,13 +1,16 @@
# pubsub - Publisher & Subscriber Package
Minimal Publisher & Subscriber Template for ROS2.
# pubsub - ROS2 Package
Python Library for ROS2 Topic Publisher & Subscriber, Service Host & Client, Actions Server & Client.
Disclaimer:
```
Created for ROS Workshop 2020
Course Roverentwicklung für Explorationsaufgaben
Created for University Course "Roverentwicklung für Explorationsaufgaben"
Institute for Space Systems
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
@ -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.
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
In this workshop you will:

View File

@ -36,13 +36,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/CustomMsg1.msg"
"msg/CustomMsg2.msg"
"srv/CustomSrv1.srv"
"srv/StateMachineSrv.srv"
"srv/InclineControlSRV.srv"
"srv/MosfetControlSRV.srv"
"srv/PitchControlSRV.srv"
"srv/RotationControlSRV.srv"
"srv/SensorSRV.srv"
"srv/WatchdogSRV.srv"
"action/TestACT.action"
DEPENDENCIES builtin_interfaces
)

View File

@ -0,0 +1,8 @@
#Goal
int8 number
---
#Response/Result
bool success
---
#Feedback
int8 current_number

View File

@ -13,10 +13,14 @@
<!-- ADD THESE LINES: START HERE -->
<build_depend>builtin_interfaces</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>rosidl_default_runtime</exec_depend>

View 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()

View 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()

View 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)

View File

@ -5,7 +5,7 @@ from rclpy.node import Node
from custom_interfaces.srv import CustomSrv1
# Service Minimal Host and Client
from pubsub.pubsub_library_v3 import MinimalServiceClientAsync
from pubsub.pubsub_library_v4 import MinimalServiceClientAsync
def main(args=None):

View File

@ -5,13 +5,13 @@ from rclpy.node import Node
from custom_interfaces.srv import CustomSrv1
# Service Minimal Host and Client
import pubsub.pubsub_library_v3 as lib
from pubsub.pubsub_library_v4 import MinimalServiceProvider
def main(args=None):
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():
try:

View File

@ -1,7 +1,7 @@
import rclpy
from rclpy.node import Node
import pubsub.pubsub_library_v3 as lib
from pubsub.pubsub_library_v4 import MinimalServiceClientAsync
# Service Type Files
from custom_interfaces.srv import StateMachineSrv
@ -10,7 +10,7 @@ from custom_interfaces.srv import StateMachineSrv
def main(args=None):
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
srv_request = StateMachineSrv.Request()

View File

@ -1,7 +1,7 @@
import rclpy
from rclpy.node import Node
from pubsub.pubsub_library_v3 import MinimalServiceProvider
from pubsub.pubsub_library_v4 import MinimalServiceProvider
# Service Type Files
from custom_interfaces.srv import StateMachineSrv

View File

@ -18,7 +18,7 @@ import rclpy
from rclpy.node import Node
# Import Subscriber Library
from .pubsub_library_v3 import MinimalSubscriber
from pubsub.pubsub_library_v4 import MinimalSubscriber
# Import Message Types
from custom_interfaces.msg import CustomMsg1

View File

@ -18,7 +18,7 @@ import rclpy
from rclpy.node import Node
# Import Publisher Library (Python Library)
from .pubsub_library import MinimalPublisher
from pubsub.pubsub_library_v4 import MinimalPublisher
# Import Message Types (Message Files)
from custom_interfaces.msg import CustomMsg1

View File

@ -25,7 +25,10 @@ setup(
'srvhost = pubsub.service_host:main',
'srvclient = pubsub.service_client: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',
],
},
)