using yocs-velocity-smoother with epuck_driver

asked 2020-06-02 11:59:58 -0500

yabdulra gravatar image

I'm using ROS on my epuck2 robot, wrote a python obstacle avoidance node. full code below

        #!/usr/bin/env python

import rospy
from sensor_msgs.msg import LaserScan
from geometry_msgs.msg import Twist

def callback_laser(msg):
    regions = {
        'right': min(min(msg.ranges[:3]), 0.07),
        'fright': min(min(msg.ranges[4:7]), 0.07),
        'front': min(min(msg.ranges[8:10]), 0.07),
        'fleft': min(min(msg.ranges[11:14]), 0.07),
        'left': min(min(msg.ranges[15:18]), 0.07),
    }
    take_action(regions)

def take_action(regions):
    msg = Twist()
    linear_x = ''
    angular_z = ''
    state_description = ''


    if regions['front'] > 0.06 and regions['fleft'] > 0.06 and regions['fright'] > 0.06:
        state_description = 'case 1 - nothing'
        linear_x = 1.0
        angular_z = 0.0
    elif regions['front'] < 0.06 and regions['fleft'] > 0.06 and regions['fright'] > 0.06:
        state_description = 'case 2 - front'
        linear_x = 0.0
        angular_z = -0.3
    elif regions['front'] > 0.06 and regions['fleft'] > 0.06 and regions['fright'] < 0.06:
        state_description = 'case 3 - fright'
        linear_x = 0.0
        angular_z = 0.1
    elif regions['front'] > 0.06 and regions['fleft'] < 0.06 and regions['fright'] > 0.06:
        state_description = 'case 4 - fleft'
        linear_x = 0.0
        angular_z = -0.1
    elif regions['front'] < 0.06 and regions['fleft'] > 0.06 and regions['fright'] < 0.06:
        state_description = 'case 5 - front and fright'
        linear_x = 0.0
        angular_z = 0.1
    elif regions['front'] < 0.06 and regions['fleft'] < 0.06 and regions['fright'] > 0.06:
        state_description = 'case 6 - front and fleft'
        linear_x = 0.0
        angular_z = -0.1
    elif regions['front'] < 0.06 and regions['fleft'] < 0.06 and regions['fright'] < 0.06:
        state_description = 'case 7 - front and fleft and fright'
        linear_x = 0.0
        angular_z = -0.2
    elif regions['front'] > 0.06 and regions['fleft'] < 0.06 and regions['fright'] < 0.06:
        state_description = 'case 8 - fleft and fright'
        linear_x = 0.0
        angular_z = -0.1
    else:
        state_description = 'unknown case'
        rospy.loginfo(regions)

    rospy.loginfo(state_description)
    msg.linear.x = linear_x
    msg.angular.z = angular_z
    pub.publish(msg)

state_description = 'case 1 - nothing'
linear_x = 1.0
angular_z = 0

pub = None

def main():
    global pub
    rospy.init_node('reading_laser')
    pub = rospy.Publisher('/mobile_base/cmd_vel', Twist, queue_size=1)
    sub = rospy.Subscriber('/scan', LaserScan, callback_laser)
    rospy.Rate(20)
    rospy.spin()

if __name__ == '__main__':
    main()

The robot jerks while receiving velocity commands from the publisher, leading to error in getting exacts robot's position. I saw online that I need a velocity smoother to solve such problem, came across yocs-velocity-smoother which is said to have been used with turtlebot. I've installed the smoother using sudo apt install ros-kinetic-yocs-velocity-smoother along with yocs-msgs. However, I couldn't get it working with my epuck robot whose driver can be found here epuck_driver, for I don't understand how to remap the topics. Could anyone please help with a brief explanation on how I can get it working. Also, it is possible to write a node that'll guarantee smooth velocity ... (more)

edit retag flag offensive close merge delete