Commit c5e0caef authored by levi's avatar levi
Browse files

merged

parents eb767507 81d364c1
ros2 topic pub --rate 1 \
/nearest_obstacle geometry_msgs/msg/Point \
"{x: 2.0, y: 2.0, z: 0.0}"
\ No newline at end of file
"{x: 2.0, y: 1.0, z: 0.0}"
\ No newline at end of file
<?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>rmq_publisher</name>
<name>rmq_bridge</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="201607384@post.au.dk">Gill</maintainer>
......
......@@ -2,7 +2,7 @@
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, Vector3
from geometry_msgs.msg import Twist, Vector3, Point
from std_msgs.msg import Bool
from sensor_msgs.msg import LaserScan
from ackermann_msgs.msg import AckermannDrive
......@@ -19,17 +19,26 @@ from msg_utils.msg_conversion import msg_to_dict
class RabbitMQPublisher(Node):
def __init__(self):
super().__init__("rabbitmq_topic_forwarder")
super().__init__("rabbitmq_publisher")
self.declare_parameter('server_hostname','localhost')
self.init_rabbitmq()
<<<<<<< HEAD:src/rmq_publisher/rmq_publisher/rmq_publisher.py
self.config = {"prefix_topic_name": True, "group_values_by_topic": True}
=======
self.config = {
"prefix_topic_name": True,
"group_values_by_topic": True
}
>>>>>>> 81d364c1d272bc041d8c1bd9270e714dc3da0c30:src/rmq_bridge/rmq_bridge/rmq_publisher.py
self.forward_this = {
"cmd_simple_control": {"type": Vector3, "fields": ["x", "y", "z"]},
"nearest_obstacle": {"type": Point, "fields": ["x", "y", "z"]},
"cmd_vel": {"type": Twist, "fields": ["linear.y", "angular.z"]},
"em_stop": {"type": Bool, "fields": ["data"]},
# "em_stop": {"type": Bool, "fields": ["data"]},
# "scan": {"type": LaserScan, "fields": ["ranges"]},
"move_cmd": {"type": AckermannDrive, "fields": ["steering_angle", "speed"]},
}
......@@ -47,8 +56,6 @@ class RabbitMQPublisher(Node):
)
self.rmq_channel.queue_declare(queue=topic_name)
self.get_logger().info(f"Initialized forwarding of topic '{topic_name}'")
self.seqno = 1
def generate_cb(self, topic_name, fields_to_forward):
return lambda msg: self.on_msg_received(msg, topic_name, fields_to_forward)
......
#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist, Vector3, Point
from std_msgs.msg import Bool
from sensor_msgs.msg import LaserScan
from ackermann_msgs.msg import AckermannDrive
import datetime
import json
import base64
import time
import pika
import threading
from msg_utils.msg_conversion import msg_to_dict
# import msg_utils as util
class RabbitMQSubscriber(Node):
def __init__(self):
super().__init__("rabbitmq_subscriber")
self.init_rabbitmq()
self.forward_this = {
"em_stop": {"type": Bool, "fields": ["data"]},
}
self.topic_publishers_ = {}
for topic_name, topic_config in self.forward_this.items():
topic_msg_type = topic_config["type"]
field_list = topic_config["fields"]
self.topic_publishers_[topic_name] = self.create_publisher(
topic_msg_type,
topic_name,
qos_profile=10
)
self.get_logger().info(f"Initialized forwarding of topic '{topic_name}'")
def on_msg_received(self, channel, method, properties, body):
print("Received message from rmq:")
rmq_msg_json = body.decode('UTF-8')
# msg_json = msg_json.replace(".","__")
rmq_msg_json = rmq_msg_json.replace('"{','{')
rmq_msg_json = rmq_msg_json.replace('}"','}')
print(rmq_msg_json)
rmq_msg = json.loads(rmq_msg_json)
for topic_name, topic_config in self.forward_this.items():
topic_msg_type = topic_config["type"]
field_list = topic_config["fields"]
if any(key.startswith(topic_name) for key in rmq_msg.keys()):
print(f"Message fits config for topic {topic_name}")
print("Converting to ros-message...")
ros_msg = topic_msg_type()
for field_name in field_list:
field_key = f"{topic_name}.{field_name}"
if field_key in rmq_msg:
if ros_msg._fields_and_field_types[field_name] == 'boolean':
field_value = bool(rmq_msg[field_key])
else:
field_value = rmq_msg[field_key]
setattr(ros_msg, field_name, field_value)
print(f"Publishing ros-message on topic /{topic_name}:")
print(ros_msg)
self.topic_publishers_[topic_name].publish(ros_msg)
# print(" [x] %r:%r consumed" % (method.routing_key, body))
def start_consumer(self):
print("Initializing Connection")
try:
self.rmq_connection = pika.BlockingConnection(
pika.ConnectionParameters(host='localhost')
)
self.rmq_channel = self.rmq_connection.channel()
self.rmq_channel.exchange_declare(
exchange="fmi_digital_twin",
exchange_type='direct')
result = self.rmq_channel.queue_declare('', exclusive=True)
queue_name = result.method.queue
self.rmq_channel.queue_bind(
exchange="fmi_digital_twin",
queue=queue_name,
routing_key="desktoprobotti.from_cosim")
self.rmq_channel.basic_consume(
queue=queue_name,
on_message_callback=self.on_msg_received,
auto_ack=True)
except Exception as err:
print("RabbitMQ Initialization failed. Is rabbitmq running?")
print(err)
else:
print("RabbitMQ Initialization Complete!")
self.rmq_channel.start_consuming()
def init_rabbitmq(self):
consumer_thread = threading.Thread(target=self.start_consumer)
consumer_thread.start()
def main(args=None):
rclpy.init(args=args)
rmq_node = RabbitMQSubscriber()
rclpy.spin(rmq_node)
rclpy.shutdown()
if __name__ == "__main__":
main()
[develop]
script-dir=$base/lib/rmq_bridge
[install]
install-scripts=$base/lib/rmq_bridge
......@@ -2,7 +2,7 @@ from setuptools import setup
import os
from glob import glob
package_name = 'rmq_publisher'
package_name = 'rmq_bridge'
share_path = os.path.join('share', package_name)
setup(
......@@ -25,7 +25,8 @@ setup(
tests_require=['pytest'],
entry_points={
'console_scripts': [
"rmq_publisher = rmq_publisher.rmq_publisher:main"
"rmq_publisher = rmq_bridge.rmq_publisher:main",
"rmq_subscriber = rmq_bridge.rmq_subscriber:main"
],
},
)
[develop]
script-dir=$base/lib/rmq_publisher
[install]
install-scripts=$base/lib/rmq_publisher
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment