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 ...
Please post results from view frames (http://www.ros.org/wiki/tf/Debugging%20tools#view_frames) and the move_base params yaml files.
Hi Weiin, I posted the files and a link below. It would be great, if you can give me hint, where to look.