def __init__(self):
rospy.Subscriber('odom', Odometry, self.get_states, queue_size=10)
rospy.Subscriber('GoalIndexTopic',
GoalIndex,
self.get_goal_index,
queue_size=10)
rospy.Subscriber('GlobalPath',
GlobWaypoints,
self.get_glob_path,
queue_size=10)
rospy.Subscriber('TransGoalStatesTopic',
TransformedGoalStates,
self.get_trans_goal_states,
queue_size=1)
self.x = 0
self.y = 0
self.theta = 0
self.goalIndex = GoalIndex()
self.globwaypoints = GlobWaypoints()
self.transformed_goal_state = TransformedGoalStates()
self.NumberofWaypoints = 2600
self.CenterX = 5
self.CenterY = 0.25
self.Radius = 0.1
self.rate = rospy.Rate(10)
self.localWaypoints = LocalWaypoints()
self.pub = rospy.Publisher('LocalWaypoints',
LocalWaypoints,
queue_size=10)
def __init__(self):
self.x = 0.0
self.prevx = 0
self.prevy = 0
self.y = 0.0
self.theta = 0.0 # in radians
self.L = 0.106
self.distance = [None] * 2600
self.angularVel = AngularVelocity()
self.longVel = 0
self.thetadot = 0
self.wheelVel = 0
self.controlGain = 1.5
self.totalSteering = 0
self.waypoints = LocalWaypoints()
self.crosstrackerr = CrossTrackError()
self.rate = rospy.Rate(200)
rospy.Subscriber('odom', Odometry, self.get_states, queue_size=1)
rospy.Subscriber('LocalWaypoints',
LocalWaypoints,
self.get_ref_path,
queue_size=1)
rospy.Subscriber('cmd_vel', Twist, self.get_long_vel, queue_size=1)
self.pub = rospy.Publisher('angular_velocity',
AngularVelocity,
queue_size=1)
self.pub2 = rospy.Publisher('CrossTrackError',
CrossTrackError,
queue_size=1)
def __init__(self):
rospy.Subscriber('odom', Odometry, self.get_states, queue_size=1)
self.xpoints = [
0.25, 9, 9.141, 9.25, 9.394, 9.553, 9.65, 9.723, 9.744, 9.749,
9.75, 9.75, 9.718, 9.638, 9.54, 9.429, 9.258, 9, 1, 0.9013, 0.7908,
0.7108, 0.5981, 0.4598, 0.3428, 0.25, 0.25
]
self.ypoints = [
0.25, 0.25, 0.2633, 0.2928, 0.3618, 0.4935, 0.6264, 0.7998, 0.906,
0.967, 1, 9, 9.218, 9.384, 9.52, 9.615, 9.704, 9.75, 9.75, 9.743,
9.72, 9.692, 9.633, 9.52, 9.361, 9, 0.25
]
#self.xpoints = [0.25 ,9 ,9.553 ,9.75 ,9.75 ,9.54 ,9 ,1 ,0.4598 ,0.25 ,0.25]
#self.ypoints = [0.25 ,0.25 ,0.4935 ,1 ,9 ,9.52 ,9.75 ,9.75 ,9.52 ,9 ,0.25]
#self.xpoints = [0.25 ,9 ,9.75 ,9.75 ,9 ,1 ,0.25 ,0.25]
#self.ypoints = [0.25 ,0.25 ,1 ,9 ,9.75 ,9.75 ,9 ,0.25]
#self.xpoints = [0.25 ,9 ,9.75 ,9.75 ,9 ,6 ,5.75 ,5.75]
#self.ypoints = [0.25 ,0.25 ,1 ,3 ,3.75 ,3.75 ,4.25 ,9]
self.x = 0
self.y = 0
self.theta = 0
self.rate = rospy.Rate(3)
rospy.Subscriber('LocalWaypoints',
LocalWaypoints,
self.get_ref_path,
queue_size=1)
rospy.Subscriber('angular_velocity',
AngularVelocity,
self.get_angular_vel,
queue_size=1)
self.waypoints = LocalWaypoints()
rospy.Subscriber('GlobalPath',
GlobWaypoints,
self.get_glob_path,
queue_size=1)
self.globwaypoints = GlobWaypoints()
self.angularVelocity = []
self.LineHeadingError = []
self.headingErr = []
self.time = []
self.xhistory = []
self.yhistory = []
rospy.Subscriber('TransGoalStatesTopic',
TransformedGoalStates,
self.get_trans_goal_states,
queue_size=1)
self.transformed_goal_state = TransformedGoalStates()
self.derivtheta = 0
self.prevglobaltheta = 0
def __init__(self):
rospy.Subscriber('odom', Odometry, self.get_states, queue_size=1)
self.xpoints = [0.25, 9, 9.75, 9.75, 9, 1, 0.25, 0.25]
self.ypoints = [0.25, 0.25, 1, 9, 9.75, 9.75, 9, 0.25]
#self.xpoints = [0.25 ,9 ,9.75 ,9.75 ,9 ,6 ,5.75 ,5.75]
#self.ypoints = [0.25 ,0.25 ,1 ,3 ,3.75 ,3.75 ,4.25 ,9]
self.x = 0
self.y = 0
self.theta = 0
self.rate = rospy.Rate(3)
rospy.Subscriber('LocalWaypoints',
LocalWaypoints,
self.get_ref_path,
queue_size=1)
rospy.Subscriber('angular_velocity',
AngularVelocity,
self.get_angular_vel,
queue_size=1)
self.waypoints = LocalWaypoints()
rospy.Subscriber('GlobalPath',
GlobWaypoints,
self.get_glob_path,
queue_size=1)
self.globwaypoints = GlobWaypoints()
self.angularVelocity = []
self.LineHeadingError = []
self.headingErr = []
self.time = []
self.xhistory = []
self.yhistory = []
rospy.Subscriber('TransGoalStatesTopic',
TransformedGoalStates,
self.get_trans_goal_states,
queue_size=1)
self.transformed_goal_state = TransformedGoalStates()
self.derivtheta = 0
self.prevglobaltheta = 0
def __init__(self):
self.x = 0.0
self.prevx=0
self.prevy=0
self.y = 0.0
self.theta = 0.0 # in radians
self.lf = 0.053
self.L = 0.106
self.CurrWheelX = 0
self.CurrWheelY = 0
self.distance = [None]*2600
self.index = 10
self.slope = 0
self.n = 0
self.a = 0
self.c = 0
self.angularVel = AngularVelocity()
self.longVel = 0
self.thetadot = 0
self.wheelVel = 0
self.e = 0
self.crossTrackSteer = 0
self.headingErr = 0
self.controlGain = 1
self.waypoints = LocalWaypoints()
self.crosstrackerr= CrossTrackError()
self.rate = rospy.Rate(10)
rospy.Subscriber('odom',Odometry,self.get_states,queue_size=10)
rospy.Subscriber('LocalWaypoints',LocalWaypoints, self.get_ref_path,queue_size=10)
rospy.Subscriber('cmd_vel',Twist,self.get_long_vel,queue_size=10)
self.pub = rospy.Publisher('angular_velocity',AngularVelocity,queue_size=10)
self.pub2=rospy.Publisher('CrossTrackError',CrossTrackError,queue_size=10)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import rospy
import math
import numpy as np
from matplotlib import pyplot as plt
from nav_msgs.msg import Odometry
from geometry_msgs.msg import Twist
from tutorial_tmcn.msg import GlobWaypoints, GoalIndex, TransformedGoalStates, LocalWaypoints
from tf.transformations import euler_from_quaternion
waypoints = LocalWaypoints()
def listener():
rospy.init_node('TestLocalWaypoints', anonymous=True)
rospy.Subscriber('LocalWaypoints',
LocalWaypoints,
get_ref_path,
queue_size=5)
rospy.spin()
def get_ref_path(msg):
waypoints.localwaypointsx = msg.localwaypointsx
waypoints.localwaypointsy = msg.localwaypointsy
theta2 = np.linspace(0, 2 * np.pi, 100)