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
|
||||
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:
|
||||
|
@ -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
|
||||
)
|
||||
|
||||
|
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 -->
|
||||
|
||||
<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
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
|
||||
|
||||
# Service Minimal Host and Client
|
||||
from pubsub.pubsub_library_v3 import MinimalServiceClientAsync
|
||||
from pubsub.pubsub_library_v4 import MinimalServiceClientAsync
|
||||
|
||||
|
||||
def main(args=None):
|
||||
|
@ -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:
|
||||
|
@ -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()
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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
|
||||
|
@ -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',
|
||||
],
|
||||
},
|
||||
)
|
||||
|
Loading…
Reference in New Issue
Block a user