Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

move_base problem

Hello @all, I am still having a problem with our custom 4-wheel robot. I can move it with keyboard or ArbotixGUI an create a map, that's fine. With move_base started with launchfile and after click on 2D Nav Goal in RVIZ nothing happened. With additional started amcl node in rviz I see the computed route (blue and red) but the movement goes in wrong direction backwards.

Here my files. Maybe somebody can look over and may see an error. If you need more information or other files, please let me know.

Thank yo for your help

Michael

The robot: fh_base_control.py

!/usr/bin/env python

Stand 27.05.2013 mha

import roslib; roslib.load_manifest('fh_basecontroller') import rospy import tf import math from math import sin, cos, pi import sys import os import time

from geometry_msgs.msg import Quaternion, Twist, Point, Pose, Pose2D, PoseWithCovariance, TwistWithCovariance, Vector3 from nav_msgs.msg import Odometry from std_msgs.msg import String from sensor_msgs.msg import JointState from EposManager.msg import MotorInfo from EposManager.msg import EPOSControl import dynamic_reconfigure.server

class FH_Basectrl(object):
def __init__(self):

    rospy.init_node('fh_basecontrol')
    rospy.loginfo("Init fh_basecontrol")

    # get values from conmon.launch
    self.ticks_per_meter = float(rospy.get_param("~ticks_per_meter", 1771))
    self.wheel_track = float(rospy.get_param("~wheel_track", 0.445))    # Distance of wheels, (Spurbreite)
    self.rate = float(rospy.get_param("~rate", 10))
    self.odom_angular_scale_correction = float(rospy.get_param("~odom_angular_scale_correction", 0.72))
    self.odom_linear_scale_correction = float(rospy.get_param("~odom_linear_scale_correction", 1))
    self.speed_factor = float(rospy.get_param("~speed_factor", 4000))   # for Eposmanager
    self.odom_frame = rospy.get_param("~odom_frame", "odom")
    self.base_frame = rospy.get_param("~base_frame", "base_link")

    now = rospy.Time.now()
    self.timeold = now
    self.t_delta = rospy.Duration(1.0 / self.rate)
    self.t_next = now + self.t_delta
    self.then = now # time for determining dx/dy

    # reset on start
    self.x = 0
    self.y = 0
    self.theta = 0
    self.d_left = 0.0
    self.d_right = 0.0
    self.motor_position_left_old = 0
    self.motor_position_right_old = 0
    self.motor_position_left_new = 0
    self.motor_position_right_new = 0

    self.init = 1   # init counter: first Motor_Infos will be trashed
    self.waitforMotor_Infos = 0     # wait untill Motor_infos from both motors arrived 

    self._pos2D  = Pose2D()   # 2D Pose for odometrie       

    # subscriptions
    self.cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
    self.motorInfoSubscriber = rospy.Subscriber("motors/Motor_Info", MotorInfo, self._motorInfoCallback)

    # publisher
    self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
    self._OdometryPublisher = rospy.Publisher("odom", Odometry)
    self._EPOSControlPublisher = rospy.Publisher("motors/Motor_Control", EPOSControl)

    self.joint_states_pub = rospy.Publisher('joint_states', JointState)     



def _motorInfoCallback(self,msg):
    """ Handle EPOS Motorinfo requests """
    self.now = rospy.Time.now()



    if msg.node_id == 1:    
        self.motor_position_left_new = msg.motor_position
        self.waitforMotor_Infos|=0x01


    if msg.node_id == 2:    
        self.motor_position_right_new = msg.motor_position  
        self.waitforMotor_Infos|=0x02


    # when both msg.motor_position arrived: go ahead
    if self.waitforMotor_Infos!=0x03:
        return

    self.waitforMotor_Infos = 0

    now = rospy.Time.now()
    if now > self.t_next:
        dt = now - self.timeold
        dt = dt.to_sec()
        self.timeold = now

    # calculate odometry
    self.d_left = (self.motor_position_left_new - self.motor_position_left_old)/ self.ticks_per_meter
    self.d_right = (self.motor_position_right_new - self.motor_position_right_old)/ self.ticks_per_meter

    # invert the direction of one motor
    self.d_right *= -1. 

    self.motor_position_left_old = self.motor_position_left_new
    self.motor_position_right_old = self.motor_position_right_new 

    # delete first values
    if self.init > 0:
        # rospy.logwarn("####### init:" + str(self.init) )
        self.init = self.init -1
        return

    dxy_ave = (self.d_left + self.d_right) / 2.0
    dtheta = (self.d_right - self.d_left) / self.wheel_track    

    vxy = dxy_ave  / dt
    vth = dtheta / dt
    dtheta = float(dtheta) / 100 # m --> cm
    dtheta = dtheta * self.odom_angular_scale_correction
    dxy_ave = float(dxy_ave)
    rospy.logwarn("d_theta: %s" %str(dtheta)) 
    rospy.logwarn("vth: %s" %str(vth))

    if (dxy_ave != 0):
        dx = cos(dtheta) * dxy_ave 
        dy = -sin(dtheta) * dxy_ave
        self.x +=((cos(self.theta) * dx - sin(self.theta) * dy) /100.) * self.odom_linear_scale_correction
        self.y +=((sin(self.theta) * -dx + cos(self.theta) * dy) /100.) * self.odom_linear_scale_correction 
    else:
        rospy.logwarn ("dxy_ave != 0")

    rospy.logwarn("pos x: %.2lf" %self.x)
    rospy.logwarn("pos y: %.2lf" %self.y)


    if dtheta != 0:
        self.theta = self.theta+dtheta


    quaternion = tf.transformations.quaternion_from_euler(0, 0, self.theta)
    quaternion = Quaternion()
    quaternion.x = 0.0
    quaternion.y = 0.0
    quaternion.z = sin(self.theta / 2.0)
    quaternion.w = cos(self.theta / 2.0)


    # First, we'll publish the transform from frame odom to frame base_link over tf
    # Note that sendTransform requires that 'to' is passed in before 'from' while
    # the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
    try:
        self._OdometryTransformBroadcaster.sendTransform(
            (self.x, self.y, 0),
            (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
            rospy.Time.now(),
            "base_link",
            "odom")               

    except Exception,e:
        rospy.logwarn("Unexpected error1:" + str(e))#str(sys.exc_info()[0]))

    odometry = Odometry()
    odometry.header.frame_id = "odom"
    odometry.child_frame_id = "base_link"
    odometry.header.stamp = now

    odometry.pose.pose.position.x = self.x
    odometry.pose.pose.position.y = self.y
    odometry.pose.pose.position.z = 0
    odometry.pose.pose.orientation = quaternion
    odometry.twist.twist.linear.x = vxy
    odometry.twist.twist.linear.y = 0
    odometry.twist.twist.angular.z = vth

    self._OdometryPublisher.publish(odometry)

def _sendmotorVelocity(self,id1,vel1):
    '''publish EPosControl msg'''
    eposctrl = EPOSControl()
    eposctrl.control_mode=EPOSControl.VELOCITY;
    eposctrl.node_id = id1;
    eposctrl.setpoint = vel1;
    rospy.loginfo(eposctrl)
    self._EPOSControlPublisher.publish(eposctrl)

def _HandleVelocityCommand(self, cmd_vel):
    """ Handle movement requests. Twistmsg"""

    v = cmd_vel.linear.x        # m/s
    omega = cmd_vel.angular.z      # rad/s

    left_speed_out  = cmd_vel.linear.x - cmd_vel.angular.z * self.wheel_track/2
    right_speed_out = cmd_vel.linear.x + cmd_vel.angular.z * self.wheel_track/2
    left_speed_out = left_speed_out * self.speed_factor
    right_speed_out = -right_speed_out * self.speed_factor

    self._sendmotorVelocity(1,left_speed_out)
    self._sendmotorVelocity(2,right_speed_out)

    message = 's %.2f %.2f\r' % (v, omega)
    rospy.logdebug("Sending speed command message: " + message)

def _HandleEposdata(self):
    rospy.loginfo(_HandleEposdata)
    data = 0
    self._BroadcastOdometryInfo(data)

def Start(self):
    rospy.logdebug("Starting")

def Stop(self):
    rospy.logdebug("Stopping")
    self._sendmotorVelocity(1,0)
    self._sendmotorVelocity(2,0)

def spin(self):
    js = JointState(name = ["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"], position=[0,0,0,0], velocity=[0,0,0,0], effort=[0,0,0,0])  
    r = rospy.Rate(self.update_rate)
    last_cmd_vel = 0, 0
    last_cmd_vel_time = rospy.get_rostime()
    last_js_time = rospy.Time(0)
    rospy.loginfo("send velocitytest")


    while not rospy.is_shutdown():
        last_time = s.header.stamp
        curr_time = rospy.get_rostime()
        js.header.stamp = curr_time + rospy.Duration(1)         


        # PUBLISH STATE

    self.odom_pub.publish(odom)
    if self.publish_tf:
        self.publish_odometry_transform(odom)
        # 1hz, future-dated joint state
    if curr_time > last_js_time + rospy.Duration(1):
        self.joint_states_pub.publish(js)
        last_js_time = curr_time

        r.sleep()

if __name__ == '__main__':
basectrl = FH_Basectrl() try: basectrl.Start()

    rospy.spin()

except rospy.ROSInterruptException:
    basectrl.Stop()
#

The launchfile:

<launch>

<env name="ROS_LOG_DIR" value="$(find EposManager)/logs"/> <rosparam command="load" file="$(find EposManager)/params/Left_Motor.yaml" ns="motors/Left_Motor"/> <rosparam command="load" file="$(find EposManager)/params/Right_Motor.yaml" ns="motors/Right_Motor"/> <node name="Drive_Motors" pkg="EposManager" type="EposManager" args="Left_Motor Right_Motor" ns="motors/" output="screen" &gt;="" <param="" name="Port" value="USB0"/>
</node>

<node name="laserscanner_tcp" pkg="rotoscan_node" type="rotoscan_node" &gt;="" <param="" name="connection" type="str" value="tcp"/> </node>

<!-- Static transform for the laser -->
<node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.12 0.0 0.2 0.0 0.0 0.0 /base_link /laser 100" />

<arg name="urdf_file" default="$(find xacro)/xacro.py '$(find fh_basecontroller)/urdf/conmon.urdf.xacro'"/>


<node name="map_server" pkg="map_server" type="map_server" args="$(find fh_basecontroller)/maps/map.yaml"/>

<node name="basecontroller" pkg="fh_basecontroller" type="fh_base_control.py"> # ueber calibrate_linear.py ermittelt # ueber calibrate_angular.py ermittelt
</node>

<node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen"> <rosparam file="$(find fh_basecontroller)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/> <rosparam file="$(find fh_basecontroller)/config/costmap_common_params.yaml" command="load" ns="local_costmap"/> <rosparam file="$(find fh_basecontroller)/config/local_costmap_params.yaml" command="load"/> <rosparam file="$(find fh_basecontroller)/config/global_costmap_params.yaml" command="load"/> <rosparam file="$(find fh_basecontroller)/config/base_local_planner_params.yaml" command="load"/>e </node>

<node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100"/>


<node name="dynamic_reconfigure" pkg="dynamic_reconfigure" type="reconfigure_gui"/>

<node name="robot_tf_publisher" pkg="conmon_tf" type="tf_broadcaster" />
<node name="robot_tf_listener" pkg="conmon_tf" type="tf_listener" />

<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
    <param name="publish_frequency" type="double" value="10.0" />
</node>

</launch>

#

rostopic list

/cmd_vel /initialpose /joint_states /map /map_metadata /motors/Group_Motor_Control /motors/Group_Motor_Info /motors/Left_Motor/parameter_descriptions /motors/Left_Motor/parameter_updates /motors/Motor_Control /motors/Motor_Info /motors/Right_Motor/parameter_descriptions /motors/Right_Motor/parameter_updates /move_base/NavfnROS/NavfnROS_costmap/inflated_obstacles /move_base/NavfnROS/NavfnROS_costmap/obstacles /move_base/NavfnROS/NavfnROS_costmap/robot_footprint /move_base/NavfnROS/NavfnROS_costmap/unknown_space /move_base/NavfnROS/plan /move_base/TrajectoryPlannerROS/cost_cloud /move_base/TrajectoryPlannerROS/global_plan /move_base/TrajectoryPlannerROS/local_plan /move_base/TrajectoryPlannerROS/parameter_descriptions /move_base/TrajectoryPlannerROS/parameter_updates /move_base/cancel /move_base/current_goal /move_base/feedback /move_base/global_costmap/inflated_obstacles /move_base/global_costmap/obstacles /move_base/global_costmap/parameter_descriptions /move_base/global_costmap/parameter_updates /move_base/global_costmap/robot_footprint /move_base/global_costmap/unknown_space /move_base/goal /move_base/local_costmap/inflated_obstacles /move_base/local_costmap/obstacles /move_base/local_costmap/parameter_descriptions /move_base/local_costmap/parameter_updates /move_base/local_costmap/robot_footprint /move_base/local_costmap/unknown_space
/move_base/parameter_descriptions
/move_base/parameter_updates
/move_base/result
/move_base/status
/move_base_simple/goal /odom /particlecloud /rosout /rosout_agg /scan /tf /waypoint_markers /waypoint_markers_array

#

rostopic echo tf:


transforms: - header: seq: 0 stamp: secs: 1370251675 nsecs: 719868000 frame_id: /map child_frame_id: /odom transform: translation: x: 0.012765523603 y: -0.00185758867047 z: 0.0 rotation: x: -0.0 y: -0.0 z: 8.56034512309e-05 w: 0.999999996336

#

rostopic echo odom:

header: seq: 2467 stamp: secs: 1370251787 nsecs: 143330097 frame_id: odom child_frame_id: base_link pose: pose: position: x: -1.01842431067 y: -0.000323505793659 z: 0.0 orientation: x: 0.0 y: 0.0 z: -0.00046593365865 w: 0.999999891453 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] twist: twist: linear: x: -7.04065560314 y: 0.0 z: 0.0 angular: x: 0.0 y: 0.0 z: -1.12850912872 covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Thank you for reading and any hint to solve the problem.

click to hide/show revision 2
corrected highlighting -.-

move_base problem

Hello @all, I am still having a problem with our custom 4-wheel robot. I can move it with keyboard or ArbotixGUI an create a map, that's fine. With move_base started with launchfile and after click on 2D Nav Goal in RVIZ nothing happened. With additional started amcl node in rviz I see the computed route (blue and red) but the movement goes in wrong direction backwards.

Here my files. Maybe somebody can look over and may see an error. If you need more information or other files, please let me know.

Thank yo for your help

Michael

The robot: fh_base_control.py

!/usr/bin/env python

#!/usr/bin/env python

# Stand 27.05.2013 mha

mha import roslib; roslib.load_manifest('fh_basecontroller') import rospy import tf import math from math import sin, cos, pi import sys import os import time

time from geometry_msgs.msg import Quaternion, Twist, Point, Pose, Pose2D, PoseWithCovariance, TwistWithCovariance, Vector3 from nav_msgs.msg import Odometry from std_msgs.msg import String from sensor_msgs.msg import JointState from EposManager.msg import MotorInfo from EposManager.msg import EPOSControl import dynamic_reconfigure.server

dynamic_reconfigure.server class FH_Basectrl(object):
def __init__(self):

__init__(self):

        rospy.init_node('fh_basecontrol')
     rospy.loginfo("Init fh_basecontrol")

     # get values from conmon.launch
     self.ticks_per_meter = float(rospy.get_param("~ticks_per_meter", 1771))
     self.wheel_track = float(rospy.get_param("~wheel_track", 0.445))    # Distance of wheels, (Spurbreite)
     self.rate = float(rospy.get_param("~rate", 10))
     self.odom_angular_scale_correction = float(rospy.get_param("~odom_angular_scale_correction", 0.72))
     self.odom_linear_scale_correction = float(rospy.get_param("~odom_linear_scale_correction", 1))
     self.speed_factor = float(rospy.get_param("~speed_factor", 4000))   # for Eposmanager
     self.odom_frame = rospy.get_param("~odom_frame", "odom")
     self.base_frame = rospy.get_param("~base_frame", "base_link")

     now = rospy.Time.now()
     self.timeold = now
     self.t_delta = rospy.Duration(1.0 / self.rate)
     self.t_next = now + self.t_delta
     self.then = now # time for determining dx/dy

     # reset on start
     self.x = 0
     self.y = 0
     self.theta = 0
     self.d_left = 0.0
     self.d_right = 0.0
     self.motor_position_left_old = 0
     self.motor_position_right_old = 0
     self.motor_position_left_new = 0
     self.motor_position_right_new = 0

     self.init = 1   # init counter: first Motor_Infos will be trashed
     self.waitforMotor_Infos = 0     # wait untill Motor_infos from both motors arrived 

     self._pos2D  = Pose2D()   # 2D Pose for odometrie       

     # subscriptions
     self.cmd_vel_sub = rospy.Subscriber("cmd_vel", Twist, self._HandleVelocityCommand)
     self.motorInfoSubscriber = rospy.Subscriber("motors/Motor_Info", MotorInfo, self._motorInfoCallback)

     # publisher
     self._OdometryTransformBroadcaster = tf.TransformBroadcaster()
     self._OdometryPublisher = rospy.Publisher("odom", Odometry)
     self._EPOSControlPublisher = rospy.Publisher("motors/Motor_Control", EPOSControl)

     self.joint_states_pub = rospy.Publisher('joint_states', JointState)     



 def _motorInfoCallback(self,msg):
     """ Handle EPOS Motorinfo requests """
     self.now = rospy.Time.now()



     if msg.node_id == 1:    
         self.motor_position_left_new = msg.motor_position
         self.waitforMotor_Infos|=0x01


     if msg.node_id == 2:    
         self.motor_position_right_new = msg.motor_position  
         self.waitforMotor_Infos|=0x02


     # when both msg.motor_position arrived: go ahead
     if self.waitforMotor_Infos!=0x03:
         return

     self.waitforMotor_Infos = 0

     now = rospy.Time.now()
     if now > self.t_next:
         dt = now - self.timeold
         dt = dt.to_sec()
         self.timeold = now

     # calculate odometry
     self.d_left = (self.motor_position_left_new - self.motor_position_left_old)/ self.ticks_per_meter
     self.d_right = (self.motor_position_right_new - self.motor_position_right_old)/ self.ticks_per_meter

     # invert the direction of one motor
     self.d_right *= -1. 

     self.motor_position_left_old = self.motor_position_left_new
     self.motor_position_right_old = self.motor_position_right_new 

     # delete first values
     if self.init > 0:
         # rospy.logwarn("####### init:" + str(self.init) )
         self.init = self.init -1
         return

     dxy_ave = (self.d_left + self.d_right) / 2.0
     dtheta = (self.d_right - self.d_left) / self.wheel_track    

     vxy = dxy_ave  / dt
     vth = dtheta / dt
     dtheta = float(dtheta) / 100 # m --> cm
     dtheta = dtheta * self.odom_angular_scale_correction
     dxy_ave = float(dxy_ave)
     rospy.logwarn("d_theta: %s" %str(dtheta)) 
     rospy.logwarn("vth: %s" %str(vth))

     if (dxy_ave != 0):
         dx = cos(dtheta) * dxy_ave 
         dy = -sin(dtheta) * dxy_ave
         self.x +=((cos(self.theta) * dx - sin(self.theta) * dy) /100.) * self.odom_linear_scale_correction
         self.y +=((sin(self.theta) * -dx + cos(self.theta) * dy) /100.) * self.odom_linear_scale_correction 
     else:
         rospy.logwarn ("dxy_ave != 0")

     rospy.logwarn("pos x: %.2lf" %self.x)
     rospy.logwarn("pos y: %.2lf" %self.y)


     if dtheta != 0:
         self.theta = self.theta+dtheta


     quaternion = tf.transformations.quaternion_from_euler(0, 0, self.theta)
     quaternion = Quaternion()
     quaternion.x = 0.0
     quaternion.y = 0.0
     quaternion.z = sin(self.theta / 2.0)
     quaternion.w = cos(self.theta / 2.0)


     # First, we'll publish the transform from frame odom to frame base_link over tf
     # Note that sendTransform requires that 'to' is passed in before 'from' while
     # the TransformListener' lookupTransform function expects 'from' first followed by 'to'.
     try:
         self._OdometryTransformBroadcaster.sendTransform(
             (self.x, self.y, 0),
             (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
             rospy.Time.now(),
             "base_link",
             "odom")               

     except Exception,e:
         rospy.logwarn("Unexpected error1:" + str(e))#str(sys.exc_info()[0]))

     odometry = Odometry()
     odometry.header.frame_id = "odom"
     odometry.child_frame_id = "base_link"
     odometry.header.stamp = now

     odometry.pose.pose.position.x = self.x
     odometry.pose.pose.position.y = self.y
     odometry.pose.pose.position.z = 0
     odometry.pose.pose.orientation = quaternion
     odometry.twist.twist.linear.x = vxy
     odometry.twist.twist.linear.y = 0
     odometry.twist.twist.angular.z = vth

     self._OdometryPublisher.publish(odometry)

 def _sendmotorVelocity(self,id1,vel1):
     '''publish EPosControl msg'''
     eposctrl = EPOSControl()
     eposctrl.control_mode=EPOSControl.VELOCITY;
     eposctrl.node_id = id1;
     eposctrl.setpoint = vel1;
     rospy.loginfo(eposctrl)
     self._EPOSControlPublisher.publish(eposctrl)

 def _HandleVelocityCommand(self, cmd_vel):
     """ Handle movement requests. Twistmsg"""

     v = cmd_vel.linear.x        # m/s
     omega = cmd_vel.angular.z      # rad/s

     left_speed_out  = cmd_vel.linear.x - cmd_vel.angular.z * self.wheel_track/2
     right_speed_out = cmd_vel.linear.x + cmd_vel.angular.z * self.wheel_track/2
     left_speed_out = left_speed_out * self.speed_factor
     right_speed_out = -right_speed_out * self.speed_factor

     self._sendmotorVelocity(1,left_speed_out)
     self._sendmotorVelocity(2,right_speed_out)

     message = 's %.2f %.2f\r' % (v, omega)
     rospy.logdebug("Sending speed command message: " + message)

 def _HandleEposdata(self):
     rospy.loginfo(_HandleEposdata)
     data = 0
     self._BroadcastOdometryInfo(data)

 def Start(self):
     rospy.logdebug("Starting")

 def Stop(self):
     rospy.logdebug("Stopping")
     self._sendmotorVelocity(1,0)
     self._sendmotorVelocity(2,0)

 def spin(self):
     js = JointState(name = ["left_wheel_joint", "right_wheel_joint", "front_castor_joint", "back_castor_joint"], position=[0,0,0,0], velocity=[0,0,0,0], effort=[0,0,0,0])  
     r = rospy.Rate(self.update_rate)
     last_cmd_vel = 0, 0
     last_cmd_vel_time = rospy.get_rostime()
     last_js_time = rospy.Time(0)
     rospy.loginfo("send velocitytest")


     while not rospy.is_shutdown():
         last_time = s.header.stamp
         curr_time = rospy.get_rostime()
         js.header.stamp = curr_time + rospy.Duration(1)         


rospy.Duration(1)


            # PUBLISH STATE

     self.odom_pub.publish(odom)
     if self.publish_tf:
         self.publish_odometry_transform(odom)
         # 1hz, future-dated joint state
     if curr_time > last_js_time + rospy.Duration(1):
         self.joint_states_pub.publish(js)
         last_js_time = curr_time

         r.sleep()

if __name__ == '__main__':
basectrl = FH_Basectrl() try: basectrl.Start()

basectrl.Start()

        rospy.spin()

 except rospy.ROSInterruptException:
     basectrl.Stop()
#

The launchfile:

<launch>

<launch>

<!-- Start the Motorcontrol -->
    <param name="run_id" value="test1" />
        <env name="ROS_LOG_DIR" value="$(find EposManager)/logs"/>
EposManager)/logs" />
        <rosparam command="load" file="$(find EposManager)/params/Left_Motor.yaml" ns="motors/Left_Motor"/>
ns="motors/Left_Motor" />
        <rosparam command="load" file="$(find EposManager)/params/Right_Motor.yaml" ns="motors/Right_Motor"/> ns="motors/Right_Motor" /> 
    <node name="Drive_Motors" pkg="EposManager" type="EposManager" args="Left_Motor Right_Motor" ns="motors/" output="screen" &gt;="" <param=""  >
        <param name="Port" value="USB0"/>
          
</node>

value="USB0" /> <param name="Protocol" value="USB" /> <param name="Publish_Rate" value="10" /> </node> <!-- Start the Laser-Node --> <node name="laserscanner_tcp" pkg="rotoscan_node" type="rotoscan_node" &gt;="" <param="" > <param name="connection" type="str" value="tcp"/> </node>

value="tcp" />
        <param name="host" type="str" value="192.168.60.3" />
        <param name="port" type="str" value="9008" />
        <param name="max_range" type="double" value="40." />
    </node>

    <!-- Static transform for the laser -->
 <node pkg="tf" type="static_transform_publisher" name="base_link_to_laser" args="0.12 0.0 0.2 0.0 0.0 0.0 /base_link /laser 100" />

/> <!-- optional Keyboard-Control --> <!-- <node name="control" pkg="drive_base_tutorial" type="drive_base" /> --> <!-- Load the URDF/Xacro model of our robot --> <arg name="urdf_file" default="$(find xacro)/xacro.py '$(find fh_basecontroller)/urdf/conmon.urdf.xacro'"/>


fh_basecontroller)/urdf/conmon.urdf.xacro'" /> <param name="robot_description" command="$(arg urdf_file)" /> <!-- Run the map server with a map --> <node name="map_server" pkg="map_server" type="map_server" args="$(find fh_basecontroller)/maps/map.yaml"/>

<!-- Start the Basecontroller with Parameters --> <node name="basecontroller" pkg="fh_basecontroller" type="fh_base_control.py"> <param name="odom_linear_scale_correction" value="1.0" /> # ueber calibrate_linear.py ermittelt <param name="odom_angular_scale_correction" value="0.72" /> # ueber calibrate_angular.py ermittelt
</node>

<param name="ticks_per_meter" value="1771" /> <param name="wheel_track" value="0.445" /> <param name="rate" value="30" /> <param name="update_rate" value="30" /> <param name="speed_factor" value = "4000" /> <param name="cmd_vel_timeout" value = "0.6" /> <param name="odom_frame" value = "odom" /> <param name="base_frame" value = "base_link" /> <param name="min_abs_yaw_vel" value = "None" /> <param name="max_abs_yaw_vel" value = "None" /> </node> <!-- Start the move_base-node --> <node name="move_base" pkg="move_base" type="move_base" respawn="false" output="screen"> <rosparam file="$(find fh_basecontroller)/config/costmap_common_params.yaml" command="load" ns="global_costmap"/> ns="global_costmap" /> <rosparam file="$(find fh_basecontroller)/config/costmap_common_params.yaml" command="load" ns="local_costmap"/> ns="local_costmap" /> <rosparam file="$(find fh_basecontroller)/config/local_costmap_params.yaml" command="load"/> command="load" /> <rosparam file="$(find fh_basecontroller)/config/global_costmap_params.yaml" command="load"/> command="load" /> <rosparam file="$(find fh_basecontroller)/config/base_local_planner_params.yaml" command="load"/>e </node>

command="load" />e </node> <node pkg="tf" type="static_transform_publisher" name="odom_map_broadcaster" args="0 0 0 0 0 0 /map /odom 100"/>


100" /> <!-- Start the GUI to reconfigure several settings --> <node name="dynamic_reconfigure" pkg="dynamic_reconfigure" type="reconfigure_gui"/>

type="reconfigure_gui" />

    <node name="robot_tf_publisher" pkg="conmon_tf" type="tf_broadcaster" />
 <node name="robot_tf_listener" pkg="conmon_tf" type="tf_listener" />

 <node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher">
     <param name="publish_frequency" type="double" value="10.0" />
 </node>
</launch>

</launch>rostopic list:

#

rostopic list

/cmd_vel
/initialpose
/joint_states
/map
/map_metadata
/motors/Group_Motor_Control
/motors/Group_Motor_Info
/motors/Left_Motor/parameter_descriptions
/motors/Left_Motor/parameter_updates
/motors/Motor_Control
/motors/Motor_Info
/motors/Right_Motor/parameter_descriptions
/motors/Right_Motor/parameter_updates
/move_base/NavfnROS/NavfnROS_costmap/inflated_obstacles
/move_base/NavfnROS/NavfnROS_costmap/obstacles
/move_base/NavfnROS/NavfnROS_costmap/robot_footprint
/move_base/NavfnROS/NavfnROS_costmap/unknown_space
/move_base/NavfnROS/plan
/move_base/TrajectoryPlannerROS/cost_cloud
/move_base/TrajectoryPlannerROS/global_plan
/move_base/TrajectoryPlannerROS/local_plan
/move_base/TrajectoryPlannerROS/parameter_descriptions
/move_base/TrajectoryPlannerROS/parameter_updates
/move_base/cancel
/move_base/current_goal
/move_base/feedback
/move_base/global_costmap/inflated_obstacles
/move_base/global_costmap/obstacles
/move_base/global_costmap/parameter_descriptions
/move_base/global_costmap/parameter_updates
/move_base/global_costmap/robot_footprint
/move_base/global_costmap/unknown_space
/move_base/goal
/move_base/local_costmap/inflated_obstacles
/move_base/local_costmap/obstacles
/move_base/local_costmap/parameter_descriptions
/move_base/local_costmap/parameter_updates
/move_base/local_costmap/robot_footprint
/move_base/local_costmap/unknown_space 
/move_base/parameter_descriptions
/move_base/parameter_updates
/move_base/result
/move_base/status
/move_base_simple/goal /odom /particlecloud /rosout /rosout_agg /scan /tf /waypoint_markers /waypoint_markers_array

#
/waypoint_markers_array

rostopic echo tf:


---
transforms: 
  - 
    header: 
      seq: 0
      stamp: 
        secs: 1370251675
        nsecs: 719868000
      frame_id: /map
    child_frame_id: /odom
    transform: 
      translation: 
        x: 0.012765523603
        y: -0.00185758867047
        z: 0.0
      rotation: 
        x: -0.0
        y: -0.0
        z: 8.56034512309e-05
        w: 0.999999996336

#

0.999999996336

rostopic echo odom:

---
header: 
  seq: 2467
  stamp: 
    secs: 1370251787
    nsecs: 143330097
  frame_id: odom
child_frame_id: base_link
pose: 
  pose: 
    position: 
      x: -1.01842431067
      y: -0.000323505793659
      z: 0.0
    orientation: 
      x: 0.0
      y: 0.0
      z: -0.00046593365865
      w: 0.999999891453
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
twist: 
  twist: 
    linear: 
      x: -7.04065560314
      y: 0.0
      z: 0.0
    angular: 
      x: 0.0
      y: 0.0
      z: -1.12850912872
  covariance: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

0.0]

Thank you for reading and any hint to solve the problem.