def __init__(self):
try:
hostname = rospy.get_param('~hostname')
except KeyError:
rospy.logfatal('[%s] parameter "hostname" is not set!',
rospy.get_name())
sys.exit(-1)
port = rospy.get_param('~port', 9090)
global tf_prefix
tf_prefix = rospy.get_param('~tf_prefix', '').strip('/')
rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(),
hostname, port)
self.robot = rosbridge.RosbridgeSetup(hostname, port)
r = rospy.Rate(10)
i = 1
while not self.robot.is_connected():
if rospy.is_shutdown():
sys.exit(0)
if self.robot.is_errored():
rospy.logfatal('[%s] connection error to %s:%i, giving up!',
rospy.get_name(), hostname, port)
sys.exit(-1)
if i % 10 == 0:
rospy.logwarn('[%s] still waiting for connection to %s:%i...',
rospy.get_name(), hostname, port)
i += 1
r.sleep()
rospy.loginfo('[%s] ... connected.', rospy.get_name())
topics = self.get_topics()
published_topics = [
topic_name for (topic_name, _, has_publishers, _) in topics
if has_publishers
]
subscribed_topics = [
topic_name for (topic_name, _, _, has_subscribers) in topics
if has_subscribers
]
for pub_topic in PUB_TOPICS:
PublisherWrapper(pub_topic, self.robot)
if ('/' + pub_topic.topic) not in published_topics:
rospy.logwarn("[%s] topic '%s' is not published by the MiR!",
rospy.get_name(), pub_topic.topic)
for sub_topic in SUB_TOPICS:
SubscriberWrapper(sub_topic, self.robot)
if ('/' + sub_topic.topic) not in subscribed_topics:
rospy.logwarn(
"[%s] topic '%s' is not yet subscribed to by the MiR!",
rospy.get_name(), sub_topic.topic)
def __init__(self):
try:
hostname = rospy.get_param('~hostname')
except KeyError:
rospy.logfatal('[%s] parameter "hostname" is not set!', rospy.get_name())
sys.exit(-1)
port = rospy.get_param('~port', 9090)
global tf_prefix
tf_prefix = rospy.get_param('~tf_prefix', '').strip('/')
rospy.loginfo('[%s] trying to connect to %s:%i...', rospy.get_name(), hostname, port)
self.robot = rosbridge.RosbridgeSetup(hostname, port)
r = rospy.Rate(10)
i = 1
while not self.robot.is_connected():
if rospy.is_shutdown():
sys.exit(0)
if self.robot.is_errored():
rospy.logfatal('[%s] connection error to %s:%i, giving up!', rospy.get_name(), hostname, port)
sys.exit(-1)
if i % 10 == 0:
rospy.logwarn('[%s] still waiting for connection to %s:%i...', rospy.get_name(), hostname, port)
i += 1
r.sleep()
rospy.loginfo('[%s] ... connected.', rospy.get_name())
topics = self.get_topics()
published_topics = [topic_name for (topic_name, _, has_publishers, _) in topics if has_publishers]
subscribed_topics = [topic_name for (topic_name, _, _, has_subscribers) in topics if has_subscribers]
for pub_topic in PUB_TOPICS:
PublisherWrapper(pub_topic, self.robot)
absolute_topic = '/' + pub_topic.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
if absolute_topic not in published_topics:
rospy.logwarn("[%s] topic '%s' is not published by the MiR!", rospy.get_name(), pub_topic.topic)
for sub_topic in SUB_TOPICS:
SubscriberWrapper(sub_topic, self.robot)
absolute_topic = '/' + sub_topic.topic.lstrip('/') # ensure exactly 1 leading slash for MiR comm
if absolute_topic not in subscribed_topics:
rospy.logwarn("[%s] topic '%s' is not yet subscribed to by the MiR!", rospy.get_name(), sub_topic.topic)
# At least with software version 2.8 there were issues when forwarding a simple goal to the robot
# This workaround converts it into an action. Check https://github.com/dfki-ric/mir_robot/issues/60 for details.
self._move_base_client = SimpleActionClient('move_base', move_base_msgs.msg.MoveBaseAction)
rospy.Subscriber("move_base_simple/goal", geometry_msgs.msg.PoseStamped, self._move_base_simple_goal_callback)