def driveForwardDistance():
global positions, accTickDistance, gpsDistance
i = 1
steering_cmd = NormalizedSteeringCommand()
steering_cmd.value = 0.0 #left 1.0 , right -1.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 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 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 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 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 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 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 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 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)
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 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 publish():
pub_speed = rospy.Publisher('/actuators/speed',
SpeedCommand,
queue_size=10)
pub_steering = rospy.Publisher("/actuators/steering_normalized",
NormalizedSteeringCommand,
queue_size=10)
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
steering_msg = NormalizedSteeringCommand()
steering_msg.value = 1
speed_msg = SpeedCommand()
speed_msg.value = 0.3
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))
rospy.sleep(2 / 0.3)
speed_msg.value = 0
pub_speed.publish(speed_msg)
rospy.sleep(10)
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("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 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 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()
#!/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)
#!/usr/bin/env python
import rospy
from autominy_msgs.msg import SpeedCommand
from autominy_msgs.msg import NormalizedSteeringCommand
rospy.init_node('publisher', anonymous=True)
pub = rospy.Publisher('/actuators/steering_normalized',
NormalizedSteeringCommand,
queue_size=10)
pub2 = rospy.Publisher('/actuators/speed', SpeedCommand, queue_size=10)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
str = NormalizedSteeringCommand()
str.value = 1.0
speed = SpeedCommand()
speed.value = 0.3
rospy.loginfo(str)
rospy.loginfo(speed)
pub.publish(str)
pub2.publish(speed)
def on_shutdown(self):
speed_msg = SpeedCommand()
speed_msg.value = 0.0
self.speed_pub.publish(speed_msg)
#!/usr/bin/env python
import rospy
from autominy_msgs.msg import NormalizedSteeringCommand
from autominy_msgs.msg import SpeedCommand
#Initialize node
rospy.init_node("rplidar_driver_node")
#Initialize publisher
publisher = rospy.Publisher("/actuators/steering_normalized",
NormalizedSteeringCommand)
publisher2 = rospy.Publisher("/actuators/speed", SpeedCommand)
while not rospy.is_shutdown():
msg = NormalizedSteeringCommand()
msg.value = 1.0
publisher.publish(msg)
msg2 = SpeedCommand()
msg2.value = 0.3
publisher2.publish(msg2)
rospy.sleep(0.5)
def drive(distance, speed, angle):
global is_active
global ticks
rospy.loginfo("%s: Running (%f)", rospy.get_caller_id(), 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
speed_cmd = SpeedCommand()
speed_cmd.value = 0
pub_speed.publish(speed_cmd)
rospy.sleep(1)
#set steering angle + speed
steering_cmd = NormalizedSteeringCommand()
steering_cmd.value = angle
pub_steering.publish(steering_cmd)
rospy.sleep(1)
last_pos = last_ceiling.pose.pose.position
current_distance = 0
speed_cmd.value = speed
pub_speed.publish(speed_cmd)
distance_per_round = 0
abs_distance = 0
rounds = 0
if angle == 0.0:
num_rounds = 1
else:
num_rounds = 5
while not rospy.is_shutdown():
current_pos = last_ceiling.pose.pose.position
current_distance = sqrt(
(current_pos.x - last_pos.x)**2 + (current_pos.y - last_pos.y)**2)
if current_distance > epsilon:
distance_per_round += current_distance
abs_distance += current_distance
last_pos = current_pos
rospy.loginfo("current distance = %f", distance_per_round)
#drive for num_rounds rounds and reset distance after 1 meter
if distance_per_round >= 1.0:
rounds += 1
distance_per_round = 0
if rounds >= num_rounds:
break
rospy.sleep(0.1)
speed_cmd.value = 0
pub_speed.publish(speed_cmd)
is_active = False
ratio = ticks/abs_distance
pub_info.publish("FINISHED")
rospy.sleep(0.3)
rospy.loginfo(
"%s: Finished (%f)\nActual travelled distance = %f\nTicks = %f\nTicks per meter: %f",
rospy.get_caller_id(),
distance,
abs_distance,
ticks, ratio)
(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:
speed_cmd = SpeedCommand()