Merge pull request #279 from fmessmer/fix/emulation

reliable start

Author: fmessmer

cob_hardware_emulation/scripts/emulation_base.py

@@ -27,9 +27,6 @@ def __init__(self, odom_frame):
 
         self.odom_frame_ = odom_frame
 
-        rospy.Subscriber("/base/twist_controller/command", Twist, self.twist_callback, queue_size=1)
-        self.pub_odom = rospy.Publisher("/base/odometry_controller/odometry", Odometry, queue_size=1)
-        rospy.Service("/base/odometry_controller/reset_odometry", Trigger, self.reset_odometry)
         self.br = tf2_ros.TransformBroadcaster()
 
         self.timestamp_last_update = rospy.Time.now()
@@ -42,6 +39,9 @@ def __init__(self, odom_frame):
         self.odom.child_frame_id = "base_footprint"
         self.odom.pose.pose.orientation.w = 1 # initialize orientation with a valid quaternion
 
+        self.pub_odom = rospy.Publisher("/base/odometry_controller/odometry", Odometry, queue_size=1)
+        rospy.Subscriber("/base/twist_controller/command", Twist, self.twist_callback, queue_size=1)
+        rospy.Service("/base/odometry_controller/reset_odometry", Trigger, self.reset_odometry)
         rospy.Timer(rospy.Duration(0.1), self.timer_cb)
 
         rospy.loginfo("Emulation for base running")

cob_hardware_emulation/scripts/emulation_nav.py

@@ -34,7 +34,6 @@ def __init__(self, odom_frame):
         self._listener = tf2_ros.TransformListener(self._buffer)
         self._transform_broadcaster = tf2_ros.TransformBroadcaster()
 
-        rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.initalpose_callback, queue_size=1)
         initialpose = rospy.get_param("~initialpose", None)
         if type(initialpose) == list:
             rospy.loginfo("using initialpose from parameter server: %s", str(initialpose))
@@ -51,6 +50,7 @@ def __init__(self, odom_frame):
         quat = tf.transformations.quaternion_from_euler(0, 0, initialpose[2])
         self._odom_transform.rotation = Quaternion(*quat)
 
+        rospy.Subscriber("/initialpose", PoseWithCovarianceStamped, self.initalpose_callback, queue_size=1)
         rospy.Timer(rospy.Duration(0.04), self.timer_cb)
 
         rospy.loginfo("Emulation for navigation running")
@@ -68,6 +68,7 @@ def __init__(self, odom_frame):
         else:
             rospy.logwarn("Emulation running without move_base due to invalid value for parameter move_base_mode: '%s'", self._move_base_mode)
 
+
     def initalpose_callback(self, msg):
         rospy.loginfo("Got initialpose, updating %s transformation.", self._odom_frame)
         try:

cob_hardware_emulation/scripts/emulation_odom_laser.py

@@ -23,16 +23,15 @@ def __init__(self, odom_frame):
 
         self._odom_frame = odom_frame
 
-        rospy.Subscriber("/base/odometry_controller/odometry", Odometry, self.odometry_callback, queue_size=1)
-        self._odom_publisher = rospy.Publisher("/scan_odom_node/scan_odom/odometry", Odometry, queue_size=1)
-
         self._transform_broadcaster = tf2_ros.TransformBroadcaster()
 
         self._odom = Odometry()
         self._odom.header.frame_id = self._odom_frame
         self._odom.child_frame_id = "base_footprint"
         self._odom.pose.pose.orientation.w = 1 # initialize orientation with a valid quaternion
 
+        self._odom_publisher = rospy.Publisher("/scan_odom_node/scan_odom/odometry", Odometry, queue_size=1)
+        rospy.Subscriber("/base/odometry_controller/odometry", Odometry, self.odometry_callback, queue_size=1)
         rospy.loginfo("Emulation for laser odometry running")
 
     def odometry_callback(self, msg):