RoverLehre
/
ROS2_pubsub
2
0
Fork 0
Code Issues 11 Pull Requests Projects Releases Wiki Activity

Compare commits

base: RoverLehre:library_v4
Branches Tags
RoverLehre:master
RoverLehre:library_v4
RoverLehre:srv_tests
..
compare: RoverLehre:master
Branches Tags
RoverLehre:library_v4
RoverLehre:master
RoverLehre:srv_tests

No commits in common. "library_v4" and "master" have entirely different histories.
library_v4 ... master

14 changed files with 22 additions and 610 deletions
Show Stats Expand all files Collapse all files

21
README.md
View File

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

8
src/custom_interfaces/CMakeLists.txt
View File

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

8
src/custom_interfaces/action/TestACT.action
View File

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

4
src/custom_interfaces/package.xml
View File

@ -13,14 +13,10 @@
<!-- 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>

58
src/pubsub/pubsub/action_client_example.py
View File

@ -1,58 +0,0 @@
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
View File

@ -1,68 +0,0 @@
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
View File

@ -1,444 +0,0 @@
#!/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)

2
src/pubsub/pubsub/service_client.py
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_v4 import MinimalServiceClientAsync
from pubsub.pubsub_library_v3 import MinimalServiceClientAsync
def main(args=None):

4
src/pubsub/pubsub/service_host.py
View File

@ -5,13 +5,13 @@ from rclpy.node import Node
from custom_interfaces.srv import CustomSrv1
# Service Minimal Host and Client
from pubsub.pubsub_library_v4 import MinimalServiceProvider
import pubsub.pubsub_library_v3 as lib
def main(args=None):
rclpy.init(args=args)
minimal_service = MinimalServiceProvider(NODE_NAME="Srv_host_test_node", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1)
minimal_service = lib.MinimalServiceProvider(NODE_NAME="Srv_host_test_node", SRV_NAME="test_srv", SRV_TYPE=CustomSrv1)
while rclpy.ok():
try:

4
src/pubsub/pubsub/statemachine_client_example.py
View File

@ -1,7 +1,7 @@
import rclpy
from rclpy.node import Node
from pubsub.pubsub_library_v4 import MinimalServiceClientAsync
import pubsub.pubsub_library_v3 as lib
# 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 = MinimalServiceClientAsync(NODE_NAME="client_cmd_srv_node", SRV_NAME="cmd", SRV_TYPE=StateMachineSrv)
client_cmd_srv = lib.MinimalServiceClientAsync(NODE_NAME="client_cmd_srv_node", SRV_NAME="cmd", SRV_TYPE=StateMachineSrv)
# Create request
srv_request = StateMachineSrv.Request()

2
src/pubsub/pubsub/statemachine_host_example.py
View File

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

2
src/pubsub/pubsub/topic_listener.py
View File

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

2
src/pubsub/pubsub/topic_talker.py
View File

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

5
src/pubsub/setup.py
View File

@ -25,10 +25,7 @@ setup(
'srvhost = pubsub.service_host:main',
'srvclient = pubsub.service_client:main',
'smhost = pubsub.statemachine_host_example:main',
'smclient = pubsub.statemachine_client_example:main',
'actclient = pubsub.action_client_example:main',
'actserver = pubsub.action_server_example:main',
'actsub = pubsub.sub:main',
'smclient = pubsub.statemachine_client_example:main'
],
},
)