Getting started

[vsd550]

Hi! I am a beginner an am doing a project that involves controlling a Pioneer 3AT using ROS . It does not have a laser ,but has SONAR and camera. I have read the basics of ROS, but am not sure how to go about controlling the robot with ROS and also take the sonar data for obstacle avoidance algorithm. Please share some study material or guidelines on how to do it? Thanks

[danimtb]

Hi @vsd550!

First of all you should start with the basics and try Rosaria node to get odometry data and to be able to pusblish velocity commands to _cmdvel topic in order to teleoperate your robot.

If you are using the _pioneerutils package from this repo, you should first launch rosaria.launch to get it up and running, then you can control the robot with the teleoperation node from another terminal by typing:

$ rosrun pioneer_utils teleop_p3at

Please, ensure you have this working and then I would try to help you with the sonar and navigation issues. Hope this helps to get started!

[vsd550]

Hi! Thanks that worked.Now i am able to operate it using the keyboard. Moving on, Please help me on how to achieve path tracking with obstacle avoidance using data from the sonar? Thanks

[vsd550]

@danimtb Please reply I am stuck on the project Thanks that worked.Now i am able to operate it using the keyboard. Moving on, Please help me on how to achieve path tracking with obstacle avoidance using data from the sonar? Thanks

[danimtb]

Hi @vsd550

To continue with navigation stack first you need to get sonar data pusblish to something like sonar topic. Are you using the sonar array in the pioneer or is it an external sensor?

[vsd550]

I am using the sonar array in the Pioneer robot itself. No external sensor. Thanks

On 17 Jun 2017 20:37, "Daniel" notifications@github.com wrote:

Hi @vsd550 https://github.com/vsd550

To continue with navigation stack first you need to get sonar data pusblish to something like sonar topic. Are you using the sonar array in the pioneer or is it an external sensor?

— You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub https://github.com/danimtb/pioneer3at_ETSIDI/issues/3#issuecomment-309220477, or mute the thread https://github.com/notifications/unsubscribe-auth/AcEDaDumPPMtIWvBE04_V6_U8HtRpZweks5sE-udgaJpZM4N5nr5 .

[danimtb]

Ok, Now ensure you get data published into _sonar (sensormsgs/PointCloud) topic after you run rosaria node. This is the data needed to perform navigation

[vsd550]

Hi @danimtb , I have a code that implements a control law for robot navigation with obstacle avoidance. Its following in Python

!/usr/bin/env python

import getch import roslib; roslib.load_manifest('single_robot_tracking') import rospy import tf import math import thread

from nav_msgs.msg import Path from nav_msgs.msg import Odometry from geometry_msgs.msg import Twist from geometry_msgs.msg import Pose from geometry_msgs.msg import PoseStamped from sensor_msgs.msg import PointCloud

rospy.init_node("single_robot_tracking")

f = open('sonar_data','w') g = open('position_data','w') twist = Twist() pub = rospy.Publisher('/RosAria/cmd_vel',Twist) path_pub = rospy.Publisher('My_path', Path, queue_size=100) last_time = rospy.get_rostime().to_sec() counter = 0 old_xr = 0 old_yr = 0 old_thr = 0 px=0 py=0 pz=0 Infinite = 1000000 obstacle_points = [Infinite]*16 yaw = 0 my_path = Path()

def give_vel(a,b): twist.linear.x = a twist.angular.z = b pub.publish(twist)

def sonar_reading(data): i =0
while(i<16): global opx global opy global opz global yaw opx = round(data.points[i].x,2) opy = round(data.points[i].y,2) opz = round(data.points[i].z,2) r = round(math.sqrt(opxopx + opyopy),2) global obstacle_points
obstacle_points[i] = r val = (i,(opx, opy, opz),r,yaw) s = str(val)
f.write(s+'\n')
i = i+1 f.write('\n')

last_time = rospy.get_rostime().to_nsec()

def pose_reading(p): global px global py global pz global yaw global my_path px = round(p.pose.pose.position.x,2) py = round(p.pose.pose.position.y,2) pz = round(p.pose.pose.position.z,2) q = (p.pose.pose.orientation.x, p.pose.pose.orientation.y, p.pose.pose.orientation.z, p.pose.pose.orientation.w) euler = tf.transformations.euler_from_quaternion(q) yaw=round(euler[2],2)

val = (yaw,(px,py,pz))  
s = str(val)    
g.write(s + '\n')
single_robot_tracking(px,py,yaw,0.4,0.2)
path_pub.publish(my_path)

pi = 3.14

def circle(r, p): points = [] i = 0
while(i<p+1): x = round(rmath.sin(i(2pi/p)), 2) y = round(r - rmath.cos(i(2pi/p)), 2) points.append((x,y)) i = i+1 return points

def find_yaw(a,b,c,d,e): y = (d-b) x = (c-a)

if (x==0):
    if (y==0):
        return e
    else:
        return (pi/2)

th = round(math.atan(abs(y/x)),2)

if ((x*y)>0):
    if (x>0):
        return th
    else:
        return (-pi + th)
else:
    if (x>0):
        return (-th)
    else:
        return (pi-th)

def help(x): return 5*x/math.pow((1+x),2)

def reference_path(vr, wr):

global my_path
my_path.header.frame_id = 'odom'
_old_xr = 0
_old_yr = 0
_old_thr = 0
_thr = 0
_xr = 0
_yr = 0
delta_t = 0
while(_old_thr<2*pi):

    _thr = _old_thr + wr*(delta_t*0.01)
    _xr = _old_xr + vr*math.cos(_old_thr)*(delta_t*0.01)
    _yr = _old_yr + vr*math.sin(_old_thr)*(delta_t*0.01) 

    pose = PoseStamped()

    quaternion = tf.transformations.quaternion_from_euler(0, 0, _thr)

    pose.pose.orientation.x = quaternion[0]
    pose.pose.orientation.y = quaternion[1]
    pose.pose.orientation.z = quaternion[2]
    pose.pose.orientation.w = quaternion[3]

    pose.pose.position.x = _xr
    pose.pose.position.y = _yr
    pose.pose.position.z = 0

    my_path.poses.append(pose)

    _old_xr = _xr
    _old_yr = _yr
    _old_thr = _thr

    delta_t = delta_t + 1

rospy.loginfo("Path is Printing")

def single_robot_tracking(current_x, current_y, current_th, vr, wr): global old_xr global old_yr global old_thr global last_time

delta_t = round((rospy.get_rostime().to_nsec() - last_time)*0.000000001, 2)
thr = old_thr + wr*(delta_t)
xr = old_xr + vr*math.cos(old_thr)*(delta_t)
yr = old_yr + vr*math.sin(old_thr)*(delta_t) 

ex = xr - current_x 
ey = yr - current_y
eth = thr - current_th
e1 = ex*math.cos(current_th) + ey*math.sin(current_th)
e2 = ex*(-math.sin(current_th)) + ey*math.cos(current_th)
e3 = eth
while(e3<-pi):
    e3+=2*pi
while(e3>pi):
    e3-=2*pi 
k1 = 0.03
k2 = 0.3
k3 = 0.3
v = vr*math.cos(e3) + k1*(e1)
rospy.loginfo("e1: %s",e1)
rospy.loginfo("e2: %s",e2)
rospy.loginfo("e3: %s",e3)
# Obstacle Avoidance
global obstacle_points
if (v>0):   
    # Obstacle-in-front 
    x1 = min(obstacle_points[0],obstacle_points[7],obstacle_points[1],obstacle_points[2],obstacle_points[3],obstacle_points[4],obstacle_points[5],obstacle_points[6])
    # Obstacle-in-left  
    x2 = min(obstacle_points[0],obstacle_points[1], obstacle_points[2])
    # Obstacle-in-right
    x3 = min(obstacle_points[5], obstacle_points[6],obstacle_points[7])

    ex1 = math.exp((0.5-x1)/0.05)
    ex2 = math.exp((0.35-x1)/0.05)
    ex3 = math.exp((0.2-x1)/0.05)

    a = 0.1
    if (x2<x3):
        a = -0.2

    ex = round(a*(help(ex1)+ help(ex2)+ help(ex3)),2)

    rospy.loginfo("Value of ex: %s",ex)
    rospy.loginfo("Value of front_sonar: %s",x1)

    w = wr + k2*(vr*math.sin(e3)*e2)/e3 + k3*e3  + ex
if (v<0):

    # Obstacle-in-front 
    x4 = min(obstacle_points[9],obstacle_points[8],obstacle_points[15],obstacle_points[10],obstacle_points[11],obstacle_points[12],obstacle_points[13],obstacle_points[14])
    # Obstacle-in-left  
    x5 = min(obstacle_points[8],obstacle_points[9], obstacle_points[10])
    # Obstacle-in-right
    x6 = min(obstacle_points[13], obstacle_points[14],obstacle_points[15])

    ex4 = math.exp((0.5-x4)/0.05)
    ex5 = math.exp((0.35-x4)/0.05)
    ex6 = math.exp((0.2-x4)/0.05)

    a = 0.1
    if (x5<x6):
        a = -0.2

    ex = round(a*(help(ex4) + help(ex5)+ help(ex6)),2)

    rospy.loginfo("Value of ex: %s",ex)
    rospy.loginfo("Value of back_sonar: %s",x4)
    w = wr + k2*(vr*math.sin(e3)*e2)/e3 + k3*e3  + ex

give_vel(v,w)

last_time = rospy.get_rostime().to_nsec()

old_xr = xr
old_yr = yr
old_thr = thr

reference_path(0.4,0.2) rospy.Subscriber('/RosAria/sonar', PointCloud, sonar_reading)
rospy.Subscriber('/RosAria/pose', Odometry, pose_reading)

rospy.spin()

exit()

/////////////////////////// But the robot is not avoiding obstacles , If you could point out the glitches in code it would be really helpful. Thanks