first test push

This commit is contained in:
2021-01-13 15:28:01 +01:00
commit c98972e4a9
21 changed files with 1156 additions and 0 deletions

31
src/pubsub/package.xml Normal file
View File

@ -0,0 +1,31 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pubsub</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="winterhalder.p@googlemail.com">patrick</maintainer>
<license>TODO: License declaration</license>
<!-- ADD THESE LINES -->
<!-- Package not listed when using this: <exec_depend>rclpy</exec_depend> -->
<!-- <exec_depend>std_msgs</exec_depend> -->
<!-- <build_depend>std_msgs</build_depend> -->
<depend>std_msgs</depend>
<!-- CUSTOM LINE -->
<!-- This is custom for the package you depend on -->
<exec_depend>pubsub_msg</exec_depend>
<!-- END HERE -->
<test_depend>ament_copyright</test_depend>
<test_depend>ament_flake8</test_depend>
<test_depend>ament_pep257</test_depend>
<test_depend>python3-pytest</test_depend>
<export>
<build_type>ament_python</build_type>
</export>
</package>

View File

View File

@ -0,0 +1,83 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#********************************************#
# Listener Template
# Creted for:
# ROS2 Workshop 2020
# Roverentwicklung für Explorationsaufgaben
# Institute for Space Systems
# University of Stuttgart
# Created by Patrick Winterhalder
# IRS, University of Stuttgart
#********************************************#
import rclpy
from rclpy.node import Node
# Import Subscriber Library
from .pubsub_library import CustomMsg1_sub
from .pubsub_library import CustomMsg2_sub
# Import Message Types
from pubsub_msg.msg import CustomMsg1
from pubsub_msg.msg import CustomMsg2
def main(args=None):
rclpy.init(args=args)
# Start nodes here, should create object <node_name> for every node
listener_1 = CustomMsg1_sub(NODE_NAME="pubsub_listener_1", TOPIC_NAME="/chatter1",MSG_TYPE=CustomMsg1, NUM_MSGS=0)
listener_2 = CustomMsg2_sub(NODE_NAME="pubsub_listener_2", TOPIC_NAME="/chatter2",MSG_TYPE=CustomMsg2, NUM_MSGS=0)
while rclpy.ok():
try:
# Insert main looping script here...
# Receive Topics by running nodes
rclpy.spin_once(listener_1, timeout_sec=0.01)
rclpy.spin_once(listener_2, timeout_sec=0.01)
# Do sth if message was received
if listener_1.topic_received is True:
listener_1.topic_received = False # zurücksetzen
msg_1 = listener_1.return_msg()
#*********************************
# Do sth based on received message
#*********************************
if listener_2.topic_received is True:
listener_2.topic_received = False # zurücksetzen
msg_2 = listener_2.return_msg()
#*********************************
# Do sth based on received message
#*********************************
# Check if "msg_1" or "msg_2" is available as a local variable
if 'msg_1' in locals():
# Print msg_1
listener_1.print_msg()
if 'msg_2' in locals():
# Print msg_2
listener_2.print_msg()
# Here, you can now also publish/pass on the results of this script by
# using the "MinimalPublisher" class. For this please refer to "talker.py".
except (KeyboardInterrupt, SystemExit):
print("\n\nShutting down...")
# Insert "<node_name>.destroy_node()" here for all running nodes in this script
# eg:
listener_1.destroy_node()
listener_2.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

View File

@ -0,0 +1,284 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#********************************************#
# Publisher/Subscriber 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
#********************************************#
import rclpy
from rclpy.node import Node
# How to use:
# from pubsub_library import MinimalPublisher
# from pubsub_library import MinimalSubscriber
# minimal_publisher = MinimalPublisher(NODE_NAME='minimal_pub', TOPIC_NAME='user_controller', MSG_TYPE=Usercontroller, MSG_PERIOD=0.5)
# minimal_subscriber = MinimalSubscriber(NODE_NAME='minimal_sub', TOPIC_NAME='epos_feedback', MSG_TYPE=Eposreturn)
# See --> talker.py, listener.py
#******************************************************************************#
# Definition of Parent Classes
class MinimalPublisher(Node):
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, MSG_PERIOD):
self.PUBLISHER_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.PUBLISHER_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
# Init above laying class Node
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
return
def listener_callback(self, msg):
self.get_logger().info('I heard: "%s"' % msg.data)
return
#***************************************************************************#
# Child Classes inherite through super().__init__(...) from parent class
# Child's purpose is to simplify subscribing to custom messages
# For this to work you must customize "listener_callback", "return_msg" (and "print_msg")
# Exemplary Subscriber Class: Inheriting from MinimalSubscriber Class
# This class is custom made for receiving the specific msg type but uses MinimalSubscriber as foundation
# Create a new class of this type for every custom msg you want to receive, customizing the listener_callback,
# return_msg (and print_msg) functions to correspond to your message
class example_subscriber(MinimalSubscriber):
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS):
# Init MinimalSubscriber:
super().__init__(NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS)
print("\t- " + str(TOPIC_NAME))
# !!! CUSTOMIZE THESE VARIABLES TO WORK WITH YOUR MESSAGE FILE !!!
self.pos_actual = None
self.vel_actual = None
self.baffle_switch = None
self.current = None
self.temp = None
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
def listener_callback(self, msg): # Overwrites callback from inherited class
self.topic_received = True
self.pos_actual = msg.pos_actual
self.vel_actual = msg.vel_actual
self.baffle_switch = msg.baffle_switch
self.current = msg.epos_current
self.temp = msg.epos_temp
#print_msg(self) # activate to print the received data --> customize print_msg(self) !
return
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
def return_msg(self): # Extract only msg variables from "self" object
msg = self.CUSTOM_MSG()
msg.pos_actual = self.pos_actual
msg.vel_actual = self.vel_actual
msg.baffle_switch = self.baffle_switch
msg.epos_current = self.current
msg.epos_temp = self.temp
return msg
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!! (optional)
def print_msg(self):
print("[SUBSCRIBER] %s received topic:\t/%s" %(self.NODE_NAME, self.TOPIC_NAME))
string = "Received:\t"
# Check length of all transmitted value lists to be equal (equal number of parameters)
length = len(self.pos_actual)
if any(len(lst) != length for lst in [self.vel_actual, self.current, self.temp]):
print("[TRANSMISSION ERROR] Length of lists inside topic %s do not match" %(self.TOPIC_NAME))
else:
complete_list = []
complete_list.append(self.pos_actual)
complete_list.append(self.vel_actual)
complete_list.append(self.current)
complete_list.append(self.temp)
#print(complete_list)
for i in range(len(complete_list)):
for y in range(len(complete_list[0])):
string += "%.2f,\t" %(complete_list[i][y])
string += str(self.baffle_switch)
print(string)
return
#************************************************************************************#
# CustomMsg1 Subscriber Class
class CustomMsg1_sub(MinimalSubscriber):
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS):
# Init MinimalSubscriber:
super().__init__(NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS)
print("\t- " + str(TOPIC_NAME))
# CustomMsg1 Variables
self.temperature = None
self.pressure = None
self.humidity = None
return
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
def listener_callback(self, msg): # Overwrites callback from inherited class
self.topic_received = True
self.temperature = msg.temperature
self.pressure = msg.pressure
self.humidity = msg.humidity
#print_msg(self) # activate to print the received data --> customize print_msg(self) !
return
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
def return_msg(self): # Extract only msg variables from "self" object
msg = self.CUSTOM_MSG()
msg.temperature = self.temperature
msg.pressure = self.pressure
msg.humidity = self.humidity
return msg
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!! (optional)
def print_msg(self):
print("[SUBSCRIBER] %s received topic:\t/%s" %(self.NODE_NAME, self.TOPIC_NAME))
string = "Received:\t"
# Check length of all transmitted value lists to be equal (equal number of parameters)
length = len(self.temperature)
if any(len(lst) != length for lst in [self.temperature, self.pressure, self.humidity]):
print("[TRANSMISSION ERROR] Length of lists inside topic %s do not match" %(self.TOPIC_NAME))
else:
complete_list = []
complete_list.append(self.temperature)
complete_list.append(self.pressure)
complete_list.append(self.humidity)
#print(complete_list)
for i in range(len(complete_list)):
for y in range(len(complete_list[0])):
string += "%.2f,\t" %(complete_list[i][y])
print(string)
return
#************************************************************************************#
# CustomMsg2 Subscriber Class
class CustomMsg2_sub(MinimalSubscriber):
def __init__(self, NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS):
# Init MinimalSubscriber:
super().__init__(NODE_NAME, TOPIC_NAME, MSG_TYPE, NUM_MSGS)
print("\t- " + str(TOPIC_NAME))
# CustomMsg1 Variables
self.pitch_ctrl = None
self.yaw_ctrl = None
return
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
def listener_callback(self, msg): # Overwrites callback from inherited class
self.topic_received = True
self.pitch_ctrl = msg.pitch_ctrl
self.yaw_ctrl = msg.yaw_ctrl
#print_msg(self) # activate to print the received data --> customize print_msg(self) !
return
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!!
def return_msg(self): # Extract only msg variables from "self" object
msg = self.CUSTOM_MSG()
msg.pitch_ctrl = self.pitch_ctrl
msg.yaw_ctrl = self.yaw_ctrl
return msg
# !!! CUSTOMIZE THIS FUNCTION TO WORK WITH YOUR MESSAGE FILE !!! (optional)
def print_msg(self):
print("[SUBSCRIBER] %s received topic:\t/%s" %(self.NODE_NAME, self.TOPIC_NAME))
string = "Received:\t"
# Check length of all transmitted value lists to be equal (equal number of parameters)
complete_list = []
complete_list.append([self.pitch_ctrl])
complete_list.append([self.yaw_ctrl])
#print(complete_list)
for i in range(len(complete_list)):
for y in range(len(complete_list[0])):
string += "%.2f,\t" %(complete_list[i][y])
print(string)
return

View File

@ -0,0 +1,69 @@
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#********************************************#
# Talker Template
# Creted for:
# ROS2 Workshop 2020
# Roverentwicklung für Explorationsaufgaben
# Institute for Space Systems
# University of Stuttgart
# Created by Patrick Winterhalder
# IRS, University of Stuttgart
#********************************************#
import rclpy
from rclpy.node import Node
# Import Publisher Library (Python Library)
from .pubsub_library import MinimalPublisher
# Import Message Types (Message Files)
from pubsub_msg.msg import CustomMsg1
from pubsub_msg.msg import CustomMsg2
def main(args=None):
rclpy.init(args=args)
# Start nodes here, should create object <node_name> for every node
talker_1 = MinimalPublisher(NODE_NAME="pubsub_talker_1", TOPIC_NAME="/chatter1", MSG_TYPE=CustomMsg1, MSG_PERIOD=0.25)
talker_2 = MinimalPublisher(NODE_NAME="pubsub_talker_2", TOPIC_NAME="/chatter2", MSG_TYPE=CustomMsg2, MSG_PERIOD=0.5)
while rclpy.ok():
try:
# Insert main looping script here...
# Eg. create custom msgs, fill and send them
# CustomMsg1 to talker_1
msg_1 = CustomMsg1()
msg_1.temperature = [24.5, 25.5, 26.5]
msg_1.pressure = [1011.5, 1012.55, 1010.11]
msg_1._humidity = [0.002, 0.0019, 0.0021]
talker_1.timer_publish(msg_1)
# CustomMsg2 to talker_2
msg_2 = CustomMsg2()
msg_2.pitch_ctrl = 0.0
msg_2.yaw_ctrl = 0.22
talker_2.timer_publish(msg_2)
# Insert "rclpy.spin_once(<node_name>, timeout_sec=0.1)" at the end of
# "try" for every node running in this script
rclpy.spin_once(talker_1, timeout_sec=0.1)
rclpy.spin_once(talker_2, timeout_sec=0.1)
except (KeyboardInterrupt, SystemExit):
print("\n\nShutting down...")
# Insert "<node_name>.destroy_node()" here once for every node running in this script
talker_1.destroy_node()
talker_2.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()

View File

4
src/pubsub/setup.cfg Normal file
View File

@ -0,0 +1,4 @@
[develop]
script-dir=$base/lib/pubsub
[install]
install-scripts=$base/lib/pubsub

27
src/pubsub/setup.py Normal file
View File

@ -0,0 +1,27 @@
from setuptools import setup
package_name = 'pubsub'
setup(
name=package_name,
version='0.0.0',
packages=[package_name],
data_files=[
('share/ament_index/resource_index/packages',
['resource/' + package_name]),
('share/' + package_name, ['package.xml']),
],
install_requires=['setuptools'],
zip_safe=True,
maintainer='patrick',
maintainer_email='winterhalder.p@googlemail.com',
description='TODO: Package description',
license='TODO: License declaration',
tests_require=['pytest'],
entry_points={
'console_scripts': [
'talker = pubsub.talker:main',
'listener = pubsub.listener:main'
],
},
)

View File

@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_copyright.main import main
import pytest
@pytest.mark.copyright
@pytest.mark.linter
def test_copyright():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found errors'

View File

@ -0,0 +1,25 @@
# Copyright 2017 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_flake8.main import main_with_errors
import pytest
@pytest.mark.flake8
@pytest.mark.linter
def test_flake8():
rc, errors = main_with_errors(argv=[])
assert rc == 0, \
'Found %d code style errors / warnings:\n' % len(errors) + \
'\n'.join(errors)

View File

@ -0,0 +1,23 @@
# Copyright 2015 Open Source Robotics Foundation, Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
from ament_pep257.main import main
import pytest
@pytest.mark.linter
@pytest.mark.pep257
def test_pep257():
rc = main(argv=['.', 'test'])
assert rc == 0, 'Found code style errors / warnings'

View File

@ -0,0 +1,59 @@
cmake_minimum_required(VERSION 3.5)
project(pubsub_msg)
# Default to C99
if(NOT CMAKE_C_STANDARD)
set(CMAKE_C_STANDARD 99)
endif()
# Default to C++14
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
# -->
# ADD THESE LINES: START HERE
find_package(builtin_interfaces REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
find_package(rclcpp REQUIRED)
# CUSTOM LINES: CHANGE FOR YOUR FILENAMES
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/CustomMsg1.msg"
"msg/CustomMsg2.msg"
DEPENDENCIES builtin_interfaces
)
# END HERE
# <--
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# uncomment the line when a copyright and license is not present in all source files
#set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# uncomment the line when this package is not in a git repo
#set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
ament_package()

View File

@ -0,0 +1,8 @@
# Header data, eg timestamp
# Problem: "header__struct.hpp: No such file or directory"
#Header header
# Sensor Data coming back from an array of atmospheric sensors
float32[] temperature
float32[] pressure
float32[] humidity

View File

@ -0,0 +1,7 @@
# Header data, eg timestamp
# Problem: "header__struct.hpp: No such file or directory"
#Header header
# User inputs, eg. for controlling a camera mast , eg. set angles [rad]
float32 pitch_ctrl
float32 yaw_ctrl

View File

@ -0,0 +1,34 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pubsub_msg</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="winterhalder.p@googlemail.com">patrick</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<!-- ADD THESE LINES: START HERE -->
<build_depend>builtin_interfaces</build_depend>
<build_depend>rosidl_default_generators</build_depend>
<exec_depend>builtin_interfaces</exec_depend>
<exec_depend>rosidl_default_runtime</exec_depend>
<member_of_group>rosidl_interface_packages</member_of_group>
<!-- END HERE -->
<export>
<build_type>ament_cmake</build_type>
</export>
</package>