def test1():
print("test 1")
global x1, y1, position1, position2, endTicks
rospy.sleep(3)
position1 = [x1, y1]
print("start position: " + str(position1))
steering_command = NormalizedSteeringCommand()
steering_command.value = 1.0
publish_steering.publish(steering_command)
rospy.sleep(1)
speed_command = SpeedCommand()
speed_command.value = 0.2
publish_speed.publish(speed_command)
rospy.sleep(5)
speed_command.value = 0.0
publish_speed.publish(speed_command)
position2 = [x1, y1]
print("end position: " + str(position2))
print("total ticks: " + str(endTicks))
euclideanDistance = euclidean_distance(position1[0], position1[1],
position2[0], position2[1])
current_slope = calculate_slope(position1[0], position1[1], position2[0],
position2[1])
current_angle = math.atan(current_slope)
print("current angle: " + str(current_angle))
radius = (euclideanDistance / (2 * math.sin(current_angle / 2)))
print("radius: " + str(radius))
distanceDriven = radius * current_angle
print("total distance driven: " + str(distanceDriven))
ratio = distanceDriven / endTicks
print("ratio between travelled distance and counted ticks: " + str(ratio))
def PID(self):
Kp = 5
Ki = 0
Kd = 0.1
error = (self.setpoint - self.yaw)
self.acc_error = self.acc_error + error #accumulator for error
current_time = rospy.Time.now()
delta_time = (current_time - self.pre_time).to_sec()
#Kp*error Ki*area Kd*slope
PID = Kp * error + Ki * self.acc_error * delta_time + Kd * (
error - self.pre_error) / delta_time
# PID = int(round(PID))
self.pre_time = current_time
self.pre_error = error
#pid value to steering message
str_com = NormalizedSteeringCommand()
str_com.value = PID
self.str_pub.publish(str_com)
#pub speed
sp = SpeedCommand()
sp.value = 0.3
self.speed_pub.publish(sp)
def publisher():
pub_steer = rospy.Publisher('/actuators/steering_normalized',
NormalizedSteeringCommand,
queue_size=10)
pub_speed = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
'''
rospy.set_param('msg_steer', 1.0)
msg_steer = rospy.get_param("msg_steer")
rospy.loginfo(msg_steer)
pub_steer.publish(msg_steer)
rospy.set_param('msg_speed' , 0.3)
msg_speed = rospy.get_param("msg_speed")
rospy.loginfo(msg_speed)
pub_speed.publish(msg_speed)
rate.sleep()
'''
msg_steer = NormalizedSteeringCommand()
msg_steer.value = 1.0
rospy.loginfo(msg_steer)
pub_steer.publish(msg_steer)
msg_speed = SpeedCommand()
msg_speed.value = 0.3
rospy.loginfo(msg_speed)
pub_speed.publish(msg_speed)
def on_control(self, tmr):
if tmr.last_duration is None:
dt = 0.01
else:
dt = (tmr.current_expected - tmr.last_expected).to_sec()
print dt
error = self.desired_speed - self.speed.value
self.integral_error += error * dt
self.integral_error = max(self.min_i, self.integral_error)
self.integral_error = min(self.max_i, self.integral_error)
derivative_error = (error - self.last_error) / dt
self.last_error = error
pid_output = self.kp * error + self.kd * derivative_error + self.ki * self.integral_error
print("x", pid_output, self.desired_speed, self.speed.value)
pid_output += self.speed.value
pid_output = min(pid_output, 0.5)
#speed_msg = SpeedCommand()
#speed_msg.value = 0.2
#self.speed_pub.publish(speed_msg)
speed_msg = SpeedCommand()
speed_msg.value = pid_output
self.speed_pub.publish(speed_msg)
def on_control(self, tmr):
if tmr.last_duration is None:
dt = 0.01
else:
dt = (tmr.current_expected - tmr.last_expected).to_sec()
# print dt
error = self.desired_speed - self.speed
self.integral_error += error * dt
self.integral_error = max(self.min_i, self.integral_error)
self.integral_error = min(self.max_i, self.integral_error)
derivative_error = (error - self.last_error) / dt
self.last_error = error
pid_output = self.speed + self.kp * error + self.kd * derivative_error + self.ki * self.integral_error
if self.max_speed < pid_output:
pid_output = self.desired_speed
elif pid_output < self.desired_speed:
pid_output = self.desired_speed
speed_msg = SpeedCommand()
speed_msg.value = pid_output
self.speed_pub.publish(speed_msg)
def drive(distance, command, speed, angle):
global is_active
speed = speed * speed_mult
rospy.loginfo("speed is %f", speed)
rospy.loginfo("%s: Running %s(%f)", rospy.get_caller_id(), command, distance)
if distance <= 0:
rospy.logerr(
"%s: Error, distance argument has to be > 0! %f given",
rospy.get_caller_id(),
distance)
return
pub_info.publish("BUSY")
if is_active:
rospy.logwarn(
"%s: Warning, another command is still active! Please wait and try again.",
rospy.get_caller_id())
return
is_active = True
# stop the car and set desired steering angle + speed
speed_cmd = SpeedCommand()
speed_cmd.value = 0
pub_speed.publish(speed_cmd)
rospy.sleep(1)
steering_cmd = NormalizedSteeringCommand()
steering_cmd.value = angle
pub_steering.publish(steering_cmd)
rospy.sleep(1)
speed_cmd.value = speed
pub_speed.publish(speed_cmd)
start_pos = last_odom.pose.pose.position
current_distance = 0
while not rospy.is_shutdown() and current_distance < (distance - epsilon):
current_pos = last_odom.pose.pose.position
current_distance = sqrt(
(current_pos.x - start_pos.x)**2 + (current_pos.y - start_pos.y)**2)
# rospy.loginfo("current distance = %f", current_distance)
rospy.sleep(0.1)
speed_cmd.value = 0
pub_speed.publish(speed_cmd)
is_active = False
current_pos = last_odom.pose.pose.position
current_distance = sqrt((current_pos.x - start_pos.x)
** 2 + (current_pos.y - start_pos.y)**2)
pub_info.publish("FINISHED")
rospy.loginfo(
"%s: Finished %s(%f)\nActual travelled distance = %f",
rospy.get_caller_id(),
command,
distance,
current_distance)
def driver(steer_pub, speed_pub):
steer_msg = NormalizedSteeringCommand()
steer_msg.value = -1.0
steer_pub.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.3
speed_pub.publish(speed_msg)
rospy.sleep(2.5)
speed_msg.value = 0.0
speed_pub.publish(speed_msg)
def talker():
pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
pub2 = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
msg = SpeedCommand()
msg.value = 0.5
pub.publish(msg)
msg2 = NormalizedSteeringCommand()
msg2.value = 1.0
pub2.publish(msg2)
rate.sleep()
def talker():
steer_pub = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
speed_pub = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
steer_msg = NormalizedSteeringCommand()
steer_msg.value = 1.0
steer_pub.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.6
speed_pub.publish(speed_msg)
rate.sleep()
def callback(self, raw_msgs):
position = raw_msgs.pose.pose.position
x = position.x
y = position.y
current_point = [x, y]
# math transformation
orientation = raw_msgs.pose.pose.orientation
quaternion = [
orientation.x, orientation.y, orientation.z, orientation.w
]
euler = tf.transformations.euler_from_quaternion(quaternion)
self.yaw = euler[2]
def rot(yaw):
return np.array([[math.cos(yaw), -math.sin(yaw)],
[math.sin(yaw), math.cos(yaw)]])
map = myMap.Map()
lane = map.lanes[self.lane]
goal_point = lane.lookahead_point(current_point, 0.5)[0]
vector = goal_point - current_point
map_point = np.matmul(rot(-self.yaw), vector)
self.setpoint = np.arctan2(map_point[1], map_point[0])
# np.arccos(np.dot(current_point,goal_point)/(np.linalg.norm(current_point)*np.linalg.norm(goal_point)))
Kp = 2.0
Ki = 0.0
Kd = 0.2
error = self.setpoint #- self.yaw
self.acc_error = self.acc_error + error #accumulator for error
current_time = rospy.Time.now()
delta_time = (current_time - self.pre_time).to_sec()
#Kp*error Ki*area Kd*slope
PID = Kp * error + Ki * self.acc_error * delta_time + Kd * (
error - self.pre_error) / delta_time
# PID = int(round(PID))
self.pre_time = current_time
self.pre_error = error
#pid value to steering message
str_com = NormalizedSteeringCommand()
str_com.value = PID
self.str_pub.publish(str_com)
#pub speed
sp = SpeedCommand()
sp.value = 0.3
self.speed_pub.publish(sp)
def driver():
steer_pub = rospy.Publisher('/actuators/steering_normalized',
NormalizedSteeringCommand,
queue_size=10)
speed_pub = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
steer_msg = NormalizedSteeringCommand()
steer_msg.value = 1.0
steer_pub.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.3
speed_pub.publish(speed_msg)
def __init__(self):
rospy.init_node("speed_pid")
self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10)
self.localization_sub = rospy.Subscriber("/sensors/speed", SpeedCommand, self.on_speed_sense, queue_size=1)
self.speed_sub = rospy.Subscriber("/control/speed", SpeedCommand, self.on_speed_control, queue_size=1)
self.speed = SpeedCommand()
self.rate = rospy.Rate(100)
self.timer = rospy.Timer(rospy.Duration.from_sec(0.01), self.on_control)
self.kp = 1.0
self.ki = 0.0
self.kd = 0.0
self.min_i = -1.0
self.max_i = 1.0
self.integral_error = 0.0
self.last_error = 0.0
# this should be changed from a topic for future tasks
self.desired_speed = 0
rospy.on_shutdown(self.on_shutdown)
while not rospy.is_shutdown():
self.rate.sleep()
def __init__(self):
# [[x0,x1,x2,...],[y0,y1,y2,...],[z0,z1,z2,...]]
self.coordinate_history = give_n_list(3)
# used to keep track for calculating distance driven
self.last_coordinate_seen = None
rospy.init_node("assignment_5", anonymous=True)
# Subscriber gets gps from ceilings camera
# topic for Subscriber /communication/gps/<ID>
# TODO: enter correct ID
self.gps_subscriber = rospy.Subscriber("/communication/gps/5",
Odometry,
self.get_points,
queue_size=10)
# Subscriber to voltage information of motor
self.steering_angle = rospy.Subscriber(
"/sensors/arduino/steering_angle",
SteeringPWMCommand,
self.print_voltage,
queue_size=10)
# Pubisher publishes steering angle
self.steer_publisher = rospy.Publisher("/actuators/steering_pwm",
SteeringPWMCommand,
queue_size=10)
self.steer_cmd = SteeringPWMCommand()
# Publisher publishes speed
self.speed_publisher = rospy.Publisher("/actuators/speed",
SpeedCommand,
queue_size=10)
self.speed_cmd = SpeedCommand()
# The spin() keeps things going
#rospy.spin()
self.rate = rospy.Rate(10) # 10ghz
def __init__(self):
rospy.init_node("basic_publisher")
self.speed_publisher = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=1)
self.steering_publisher = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand,
queue_size=1)
self.r = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
steer_msg = NormalizedSteeringCommand()
steer_msg.value = 1.0
self.steering_publisher.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.3
self.speed_publisher.publish(speed_msg)
self.r.sleep()
def __init__(self):
print("initializing Driver")
rospy.init_node("assignment_6", anonymous = True)
self.rate = rospy.Rate(10)
self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10)
self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10)
self.steer_cmd = NormalizedSteeringCommand()
self.speed_cmd = SpeedCommand()
self.speed_sub = rospy.Subscriber("/actuators/speed", Speed, self.print_speed_given, queue_size=10)
print("initialized complete")
def callback(self,raw_msgs):
#print("call")
msg=raw_msgs.pose.pose.orientation
msg_array=[msg.x,msg.y,msg.z,msg.w]
#print(str(msg))
euler=tf.transformations.euler_from_quaternion(msg_array)
#print(euler)
steering=NormalizedSteeringCommand()
steering.value=self.pid(euler[2])
speed=SpeedCommand()
speed.value=0.5
#print(str(steering.value))
#cv2.imshow('image',img)
#cv2.waitKey(3)
self.x4.publish(speed)
try:
self.x3.publish(steering)
except CvBridgeError as e:
print(e)
def publish():
pub_speed = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
pub_steering = rospy.Publisher("/actuators/steering_normalized",
NormalizedSteeringCommand,
queue_size=10)
steering_msg = NormalizedSteeringCommand()
steering_msg.value = 1
speed_msg = SpeedCommand()
speed_msg.value = 0.3
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
pub_steering.publish(steering_msg)
pub_speed.publish(speed_msg)
rospy.loginfo('speed is ' + str(speed_msg.value))
print('speed is ' + str(speed_msg.value))
rate.sleep()
def on_control(self, tmr):
if tmr.last_duration is None:
dt = 0.01
else:
dt = (tmr.current_expected - tmr.last_expected).to_sec()
#print dt
quat = [
self.pose.pose.pose.orientation.x,
self.pose.pose.pose.orientation.y,
self.pose.pose.pose.orientation.z,
self.pose.pose.pose.orientation.w
]
roll, pitch, yaw = tf.transformations.euler_from_quaternion(quat)
print(self.desired_angle)
error = self.desired_angle - yaw
self.integral_error += error * dt
self.integral_error = max(self.min_i, self.integral_error)
self.integral_error = min(self.max_i, self.integral_error)
derivative_error = (error - self.last_error) / dt
self.last_error = error
pid_output = self.kp * error + self.kd * derivative_error + self.ki * self.integral_error
clamp_pid_out_put = max(self.min_i, pid_output)
clamp_pid_out_put = min(self.max_i, clamp_pid_out_put)
speed_msg = SpeedCommand()
if self.desired_angle > 0.1:
speed_msg.value = 0.6
else:
speed_msg.value = 1.0
self.speed_pub.publish(speed_msg)
steering_msg = NormalizedSteeringCommand()
steering_msg.value = clamp_pid_out_put
self.steering_pub.publish(steering_msg)
def main():
while curX == 0.0:
pass
pos1 = (curX,curY)
time.sleep(1)
steering_cmd = NormalizedSteeringCommand()
steering_cmd.value = 1.0
pub_steering.publish(steering_cmd)
time.sleep(1)
speed_cmd = SpeedCommand()
speed_cmd.value = 0.2
pub_speed.publish(speed_cmd)
time.sleep(4)
speed_cmd.value = 0.0
pub_speed.publish(speed_cmd)
time.sleep(2)
pos2 = (curX,curY)
speed_cmd.value = 0.2
pub_speed.publish(speed_cmd)
time.sleep(4)
speed_cmd.value = 0.0
pub_speed.publish(speed_cmd)
time.sleep(2)
pos3 = (curX,curY)
print "Radius:",berechneRadius(pos1,pos2,pos3),"Meter"
print "Winkel:",berechneEinlenkWinkel(berechneRadius(pos1,pos2,pos3)),"Grad, weil niemand was mit Bogenmass anfangen kann"
def driveCircleDistance(direction):
global positions, accTickDistance, gpsDistance
i = 1
steering_cmd = NormalizedSteeringCommand()
if (direction == "left"):
steering_cmd.value = 1.0
elif (direction == "right"):
steering_cmd.value = -1.0
else:
steering_cmd.value = 0.0
pub_steering.publish(steering_cmd)
speed = SpeedCommand()
speed.value = 0.2 #moeglichst langsam
pub_speed.publish(speed)
time.sleep(4)
while i < len(positions):
gpsDistance += math.sqrt(
math.pow(positions[i][0] - positions[i - 1][0], 2) +
math.pow(positions[i][1] - positions[i - 1][1], 2))
accTickDistance += accTick
# print(gpsDistance)
i = i + 1
rospy.loginfo("cumulativ gpsDistance (cm): " + str(gpsDistance))
rospy.loginfo("last - first position gps distance (cm) : " + str(
math.sqrt(
math.pow(positions[len(positions) - 1][0] - positions[0][0], 2) +
math.pow(positions[len(positions) - 1][1] - positions[0][1], 2))))
rospy.loginfo("Ticks : " + str(accTickDistance))
rospy.loginfo("Distance per Tick (cumulativ gpsDistance (cm)/ ticks): " +
str(gpsDistance / accTickDistance))
speed.value = 0.0
pub_speed.publish(speed)
def talker():
# Publish to this existing topics
pub_steering = rospy.Publisher('/actuators/steering_normalized', NormalizedSteeringCommand, queue_size=10)
pub_speed = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
rospy.init_node('steering_speed_whisperer', anonymous=True)
# steering
norm_steerin_cmd = NormalizedSteeringCommand() # Command is a class create object
norm_steerin_cmd.value = 0
#speed init
speed_cmd = SpeedCommand()
speed_cmd.value = 0.3
rate = rospy.Rate(5) # 5 hz
while not rospy.is_shutdown():
info_str = "time {time} : speed: {speed} , steering: {steering}".format(time=rospy.get_time(),speed=speed_cmd.value,steering=norm_steerin_cmd.value)
rospy.loginfo(info_str)
pub_steering.publish(norm_steerin_cmd)
pub_speed.publish(speed_cmd)
rate.sleep()
def __init__(self):
rospy.init_node("initNull")
self.speed = SpeedCommand()
self.steer = NormalizedSteeringCommand()
self.position = Odometry()
self.speed_pub = rospy.Publisher("/actuators/speed", SpeedCommand, queue_size=10)
self.steer_pub = rospy.Publisher("/actuators/steering_normalized", NormalizedSteeringCommand, queue_size=10)
self.position_sub = rospy.Subscriber("/communication/gps/9", Odometry, self.on_position, queue_size=10)
self.rate = rospy.Rate(100)
while not rospy.is_shutdown():
self.speed.value = 0.0
self.speed_pub.publish(self.speed)
self.steer.value = 0.0
self.steer_pub.publish(self.steer)
self.rate.sleep()
def __init__(self):
rospy.init_node("initSpeed", anonymous=True)
self.speed = SpeedCommand()
self.steer = NormalizedSteeringCommand()
self.position = Odometry()
self.speed_pub = rospy.Publisher("/actuators/speed",
SpeedCommand,
queue_size=10)
self.steer_pub = rospy.Publisher("/actuators/steering_normalized",
NormalizedSteeringCommand,
queue_size=10)
self.position_sub = rospy.Subscriber("/communication/gps/9",
Odometry,
self.on_position,
queue_size=10)
self.error_counter = 0
self.rate = rospy.Rate(10)
self.Kp = 0.0
self.Ki = 0.0
self.Kd = 0.0
self.current_time = time.time()
self.last_time = self.current_time
self.error_time = 0.0
self.error = 0.0
self.P = 0.0
self.I = 0.0
self.D = 0.0
self.PID = 0.0
self.max_integrator = 5.0
self.min_integrator = -5.0
self.speed_value = 0.5
self.steering_value = 0.0
while not rospy.is_shutdown():
self.drive()
pass
rospy.spin()
def __init__(self, desired_angle):
# #
self.desired_angle = desired_angle
self.accepted_error = 0.05 # Um die Lenkung zu entlasten
self.speed_cmd = SpeedCommand()
self.speed_cmd.value = 0.3
self.pub_speed = rospy.Publisher("/actuators/speed",
SpeedCommand,
queue_size=100)
self.PID_controller = PID_Controller()
self.pub_steering = rospy.Publisher("/actuators/steering_normalized",
NormalizedSteeringCommand,
queue_size=100)
self.gps_subscriber = rospy.Subscriber(
"/communication/gps/5", Odometry,
self.callbackGPS) # subscribe to topic "/sensor/speed
def driver():
steer_pub = rospy.Publisher('/actuators/steering_normalized',
NormalizedSteeringCommand,
queue_size=10)
speed_pub = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
steer_msg = NormalizedSteeringCommand()
steer_msg.value = 1.0
steer_pub.publish(steer_msg)
speed_msg = SpeedCommand()
speed_msg.value = 0.3
speed_pub.publish(speed_msg)
rospy.sleep(1)
speed_msg = SpeedCommand()
speed_msg.value = 0.0
speed_pub.publish(speed_msg)
#!/usr/bin/env python
import rospy
import math
from geometry_msgs.msg import Quaternion
from nav_msgs.msg import Odometry
from autominy_msgs.msg import SpeedCommand
from autominy_msgs.msg import NormalizedSteeringCommand
rospy.init_node("uebung9", anonymous=True)
kP=1.0
kD=0.0
currAngle=0.0
desiredAngle=0.0
speed_command=SpeedCommand()
steering_command=NormalizedSteeringCommand()
oldTime=rospy.get_time()
newTime=rospy.get_time()
oldError=0.0
yawAngle=input("Please input desired yaw angle: 0 for 0, 1 for pi, 2 for -pi: ")
if yawAngle==0:
desiredAngle=0
elif yawAngle==1:
desiredAngle=math.pi
else:
desiredAngle=-1*(math.pi)
def getOdometry(odometry):
def on_shutdown(self):
speed_msg = SpeedCommand()
speed_msg.value = 0.0
self.speed_pub.publish(speed_msg)
import rospy
import math
import time
import numpy as np
from nav_msgs.msg import Odometry
from geometry_msgs.msg import PointStamped
from autominy_msgs.msg import NormalizedSteeringCommand, SteeringCommand, SpeedCommand
from map import Lane, Map, MapVisualization
from steering_pid import SteeringPID
rospy.init_node("uebung11")
currx = 0.0
curry = 0.0
currAngle = 0.0
speed_msg = SpeedCommand()
steering_msg = SteeringCommand()
#from map.py
lane_1 = np.load("lane1.npy")
lane_2 = np.load("lane2.npy")
lanes = [
Lane(lane_1[[
0, 50, 209, 259, 309, 350, 409, 509, 639, 750, 848, 948, 1028, 1148,
1200, 1276
], :]),
Lane(lane_2[[
0, 50, 100, 150, 209, 400, 600, 738, 800, 850, 900, 949, 1150, 1300,
1476
], :])
]
#!/usr/bin/env python
import rospy
from autominy_msgs.msg import NormalizedSteeringCommand
from autominy_msgs.msg import SpeedCommand
PUBLISH_RATE = 10 #publish rate. The higher this is the high is the frequency of the publish
steeringCommand = NormalizedSteeringCommand()
speedCommand = SpeedCommand()
steeringCommand.value = 1.0
speedCommand.value = 0.3
def publish():
rospy.init_node('speedPublisherNode')
# publischer for steering to the wheels. Send messages with a small Queue.
steeringPublisher = rospy.Publisher('/actuators/steering_normalized',
NormalizedSteeringCommand,
queue_size=10)
# publisher for publishing messages to speed topic (updating speed)
speedPublisher = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
#turn the wheles
steeringPublisher.publish(steeringCommand)
#publish value for speed
speedPublisher.publish(speedCommand)
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
print("Xyaw: ", yaw)
print("Xpit: ", pitch)
print("Xrol: ", roll)
return yaw
if __name__ == '__main__':
rospy.init_node('PID', anonymous=True)
pub_speed = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
# speed init
speed_cmd = SpeedCommand()
speed_cmd.value = 0.0
rate = rospy.Rate(300) # 100 hz
pub_speed.publish(speed_cmd)
drive_pid = Drive_PID(0)
# while not rospy.is_shutdown():
# pub_speed.publish(speed_cmd)
# rate.sleep()
# spin() simply keeps python from exiting until this node is stopped
try:
rospy.spin()
except KeyboardInterrupt:
