Open darkobozhinoski opened 4 years ago
Just for information, who is publishing /d_obstacle
? @darkobozhinoski @marioney
Just for information, who is publishing
/d_obstacle
? @darkobozhinoski @marioney
There's a node called https://github.com/rosin-project/metacontrol_sim/blob/MVP_world/scripts/safety_distance_publisher.py
It uses filtered laser data to get the closest distance
@darkobozhinoski Thanks for the information!
Solved by: https://github.com/rosin-project/rosin-experiments/pull/5
The resulted auto-generated model file fits perfectly to the observer code:
PackageSet { package {
CatkinPackage rosgraph_monitor { artifact {
Artifact my_observer {
node Node { name /my_observer
publisher {
Publisher { name '/diagnostics' message 'diagnostic_msgs.DiagnosticArray'}}
subscriber {
Subscriber { name '/d_obstacle' message 'std_msgs.Float32'},
Subscriber { name '/odom' message 'nav_msgs.Odometry'},
Subscriber { name 'imu/data' message 'sensor_msgs.Imu'}}}}}}}}
and the autogenerated python code looks:
from rosgraph_monitor.observer import TopicObserver
from std_msgs.msg import Float32
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from diagnostic_msgs.msg import DiagnosticStatus, KeyValue
class Myobserver(TopicObserver):
def __init__(self, name):
topics = [
("/d_obstacle", Float32),
("/odom", Odometry),
("imu/data", Imu)
]
super(Myobserver, self).__init__(
name, 10, topics)
def calculate_attr(self, msgs):
status_msg = DiagnosticStatus()
#Write here your code (or uncomment the following lines)
#if len(msgs) < 2:
# print("Incorrect number of messages")
# return status_msg
#...
#attr = msgs[0].data + msgs[1].data
#print("{0} + {1}".format(msgs[0].data, msgs[1].data))
status_msg = DiagnosticStatus()
status_msg.level = DiagnosticStatus.OK
status_msg.name = self._id
# Set here your name attribute
status_msg.values.append(
KeyValue("KeyName", attr))
status_msg.message = "QA status"
return status_msg
this confirms that the observer generator works fine and the global RosSystem model is correct! :tada: :tada:
NOTE: I had to add two new nodes that correspond to gazebo plugins (the simulation of the base controllers and the imu sensor), but I think that make sense to have both there.
These are the topics to which the observer needs to subsbscirbe:
topics = [("/odom", Odometry), ("/imu/data", Imu), ("/d_obstacle", Float32)]
The code to the observer is here