def callback(data):
summation = Int16(data.a + data.b)
rospy.loginfo("The sum of {} and {} is {}".format(str(data.a), str(data.b),
str(summation)))
pub.publish(summation)
def cruise(accelPub_, brakePub_):
global velocity
global cruise_mode
global cruise_speed
accel_ = Int16()
brake_ = Int16()
prev_time = 0
prev_error = 0
error_i = 0
while(1):
if cruise_mode == 1:
#accel_ = Int16()
#brake_ = Int16()
os.system('cls' if os.name == 'nt' else 'clear')
cur_time = time.time()
del_time = cur_time - prev_time;
#
k_p = 1.25
k_i = 0.75
k_d = 0
windup_guard = 70.0
error_p = cruise_speed - velocity
error_i += error_p * del_time
# Anti wind-up
if (error_i < -windup_guard):
error_i = -windup_guard
elif (error_i > windup_guard):
error_i = windup_guard
error_d = (error_p - prev_error)/del_time
pid_out = k_p*error_p + k_i*error_i + k_d*error_d
prev_error = error_p # Feedback current error
prev_time = cur_time # Feedback current time
# accel_max - accel_dead_zone = 3000 - 800 = 2200
# 2200/10 = 220, 220 + 1 = 221
if pid_out > 0:
for i in range(221):
if i <= pid_out < i+1:
accel_.data = 800 + 10*i
brake_.data = 0
# brake_max - brake_dead_zone = 27000 - 3500 = 23500
# 23500/10 = 2350, 2350 + 1 = 2351
elif pid_out < 0:
for i in range(2351):
if i <= abs(pid_out) < i+1:
accel_.data = 0
brake_.data = 3500+10*i
# Change PID coefficient through this values
print("error_p: ", error_p)
print("error_i: ", error_i)
print("error_d: ", error_d)
print("pidout: ", pid_out)
print("desired_speed: ", cruise_speed)
print("current_speed: ", velocity)
print(accel_.data, brake_.data)
#accel_ = accel_out
brakePub_.publish(brake_)
accelPub_.publish(accel_)
time.sleep(0.1)
def anafi3(cmd):
pub = rospy.Publisher("anafi_3/master", Int16, queue_size=10)
data = Int16()
data = cmd
rospy.loginfo(data)
pub.publish(data)
def thrower(self, speed):
self.thrower_pub.publish(Int16(speed))
def publish_finished_to_map(self):
self.pub_finish.publish(Int16(self.robot_id))
return
def run(self):
while not rospy.is_shutdown():
self.lock.acquire()
self.current_time = time.time()
self.last_tiem = self.current_time
# self.zumy.cmd(0.1,0.1)
# print imu and enc data
# self.zumy.read_data()
imu_data = self.zumy.read_imu()
enc_data = self.zumy.read_enc()
# imu_data = [0,0,0,0,0,0]
# enc_data = [0,0]
self.lock.release()
twistImu = Twist()
kalman_msg = kalman()
if len(imu_data) == 6:
imu_msg = Imu()
imu_msg.header = Header(self.imu_count,rospy.Time.now(),self.name)
imu_msg.linear_acceleration.x = 9.81 * imu_data[0]
imu_msg.linear_acceleration.y = 9.81 * imu_data[1]
imu_msg.linear_acceleration.z = 9.81 * imu_data[2]
imu_msg.angular_velocity.x = 3.14 / 180.0 * imu_data[3]
imu_msg.angular_velocity.y = 3.14 / 180.0 * imu_data[4]
imu_msg.angular_velocity.z = 3.14 / 180.0 * imu_data[5]
#twistImu.angular.x = 0
#twistImu.angular.y = 0
#twistImu.angular.z = imu_msg.angular_velocity.z
self.imu_pub.publish(imu_msg)
# kalman_msg = kalman()
kalman_msg.keyboard_linear = self.v
kalman_msg.keyboard_angular = -self.a*3.14
vvv = self.v
aaa = self.a*3.14
self.angular_pub.publish(-aaa)
self.linear_pub.publish(vvv)
kalman_msg.measure_angular = imu_msg.angular_velocity.z
# self.kalman_pub.publish(kalman_msg)
self.nonekalman_angular.publish(kalman_msg.measure_angular)
if len(enc_data) == 2:
enc_msg = Int16()
enc_dif_r = Int16()
enc_dif_l = Int16()
#global enc_data_r
#global enc_data_l
self.lock.acquire()
ly_daenc = numpy.load('ly_enc.npy')
enc_msg.data = enc_data[0]
self.lock.release()
self.r_enc_pub.publish(enc_msg)
#ly_daenc[0] = enc_data[0]
self.lock.acquire()
enc_data_r = ly_daenc[0]
enc_dif_r.data = enc_msg.data - enc_data_r # difference value
enc_msg.data = enc_data[1]
self.lock.release()
self.l_enc_pub.publish(enc_msg)
#ly_daenc[1] = enc_data[1]
self.lock.acquire()
enc_data_l = ly_daenc[1]
enc_dif_l.data = enc_msg.data - enc_data_l # difference value
ly_daenc = enc_data
numpy.save('ly_enc.npy',ly_daenc)
#this get the robocar`s wheels of velocity
# velocity =(( encoder`s value of change in one cycle ) / total number of encoder) * wheel`s perimeter
enc_sr = (float(enc_dif_r.data)/120.0)*30*3.14
# enc_vr =float(enc_dif_r /1167)*17.25 # it don`t work!!!wtf
enc_vr = enc_sr * 0.01725
enc_sl = (float(enc_dif_l.data)/120.0)*30*3.14
# enc_vl =float(enc_dif_l /1167)*17.25
enc_vl = enc_sl * 0.01725
#print enc_dif_r,'dif_r'
#print enc_dif_l,'dif_l'
#print enc_sr,'sr'
#print enc_sl,'sl'
#print enc_vr,'vr'
#print enc_vl,'vl'
self.lock.release()
enc_v_v = self.enc_vel(enc_vr,enc_vl)
enc_v = enc_v_v/4.4
if self.v > self.hey or self.v < self.hey:
if enc_v == self.hey:
enc_v = self.enc_v_last
self.enc_v_last = enc_v
print self.enc_v_last ,'enc_v'
enc_r_r = self.enc_rad(enc_vr,enc_vl)
enc_r = enc_r_r/3
if self.a > self.hey or self.a < self.hey:
if enc_r == 0:
enc_r = self.enc_r_last
self.enc_r_last = enc_r
print self.enc_r_last ,'enc_r'
twistImu.linear.x = self.enc_v_last
twistImu.linear.y = 0
twistImu.linear.z = 0
twistImu.angular.x = 0
twistImu.angular.y = 0
twistImu.angular.z = self.enc_r_last
kalman_msg.linear_vel = self.enc_v_last
self.kalman_pub.publish(kalman_msg)
self.enc_angular.publish(self.enc_r_last)
self.nonekalman_linear.publish(self.enc_v_last)
self.current_time = time.time()
# time_dif = self.current_time - self.last_time
# self.last_time = self.current_time
# print time_dif,'tiem_dif'
# numpy.save('ly_enc.npy',ly_daenc)
#self.ly_twist_pub.publish(twistImu)
#enc_data_r = enc_data[0]#keep last value
# enc_data_l = enc_data[1]
# print enc_data_r
# print enc_data_l
# self.lock.acquire()
# linear_output = self.PID.update(self.enc_v_last)
# angular_output = self.PID_another.update(enc_r)
# print output,'output'
# print linear_output,'linear_output'
# v = linear_output/2
# r = v + (0.2*self.a)
# l =(v/1.15) - (0.2*self.a)
# print a,'a'
# self.lock.acquire()
# vv = v*2
# self.sudu_pub.publish(vv)
# self.lock.acquire()
# self.zumy.cmd(l,r)
# self.lock.release()
time_dif = self.current_time - self.last_time
self.last_time = self.current_time
# print time_dif,'tiem_dif'
# self.sudu_pub.publish(vv)
#self.ly_twist_pub.publish(twistImu)
self.rate.sleep()
def execute2():
global cx, cy, cyaw, ck, s
global global_path_x, global_path_y
global state
global last_idx
global x, y, yaw, v, t
global time
global accelPub_
global brakePub_
global steerPub_
global history_x, history_y
state = State(x=global_path_x[0], y=global_path_y[0])
while True:
if len(cx) > 0 and len(cy) > 0 and len(global_path_x) > 0 and len(
global_path_y) > 0:
#state = State(x=global_path_x[0], y=global_path_y[0], yaw=np.radians(20.0), v=0.0)
accel_ = Int16()
brake_ = Int16()
# accel_.data, brake_.data = PID()
target_idx, error_front_axle = calc_target_index(state, cx, cy)
di, target_idx = stanley_control(state, cx, cy, cyaw, target_idx)
msg = Int16()
state.update(di)
deaccel_delta = float(di * 180 / (math.pi))
abs_deaccel_delta = int(abs(deaccel_delta))
accel = 120
if abs_deaccel_delta >= 400:
accel_.data = accel - abs_deaccel_delta / 30
accelPub_.publish(accel_)
else:
accel_.data = accel
accelPub_.publish(accel_)
accelPub_.publish(accel_)
# brakePub_.publish(brake_)
# print("delta: ", di)
# print("error_front_axle: ", error_front_axle)
print("target_idx: ", target_idx)
# down_delta = float(di * 180/(math.pi)/1800)
# print("down_delta: ", down_delta)
# 71 = 2000/28.1690
raw_steer = int(-di * (180 / (math.pi)) * 71)
print("raw_steer: ", raw_steer)
if -2000 < raw_steer < 2000:
msg.data = raw_steer
elif raw_steer <= -2000:
msg.data = -2000
elif raw_steer >= 2000:
msg.data = 2000
print("Steer: ", msg.data)
steerPub_.publish(msg)
# deaccel_speed = deacceleration(di)
# print("deaccel_speed: ", deaccel_speed)
print("Accel: ", accel_.data)
print("Brake: ", brake_.data)
print("utm_x: ", utm_x)
print("utm_y: ", utm_y)
print("-----------------------------------------------")
#print("limitedSteer: ", msg.data)
#print("pidout: ", pid_out)
# time += dt
x.append(state.x)
y.append(state.y)
yaw.append(state.yaw)
v.append(state.v)
# t.append(time)
history_x.append(state.x)
history_y.append(state.y)
if show_animation: # pragma: no cover
plt.cla()
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect(
'key_release_event',
lambda event: [exit(0) if event.key == 'escape' else None])
# course가 local path의 역할을 함.
# course를 그릴 때 cx, cy가 필요함.
plt.plot(cx, cy, ".y", label="course")
# plt.scatter(global_path_x, global_path_y)
plt.plot(x, y, "-b", label="trajectory")
plt.plot(history_x, history_y, "-c", label="history")
try:
plt.plot(cx[target_idx],
cy[target_idx],
"xk",
label="target")
except:
print("plot error")
#plt.axis("equal")
plt.grid(True)
plt.title("Speed[km/h]:" + str(gpsvel)[:4])
plt.axis([
min(global_path_x) - 5,
max(global_path_x) + 5,
min(global_path_y) - 5,
max(global_path_y) + 5
])
# plt.axis("equal")
plt.pause(0.001)
majorLocator = MultipleLocator(20)
majorFormatter = FormatStrFormatter('%d')
minorLocator = MultipleLocator(5)
pass
#!/usr/bin/env python
#-----------------------------------------------
import rospy
from std_msgs.msg import Int16
#-----------------------------------------------
import Tkinter
from Tkinter import *
import ttk
import time
#------------------------------------------------
X = Int16()
Y = Int16()
x = Int16()
y = Int16()
#-----------------------------------------------
def callback_x(data1):
global X
X.data = data1.data
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data1)
def callback_y(data2):
global Y
Y.data = data2.data
rospy.loginfo(rospy.get_caller_id() + "I heard %s", data2)
def task_publisher(self, task):
self.pub.publish(Int16(task))
return
job.job_goal.header.frame_id = "map"
job.job_goal.pose.position.x = 2 # 11.4
job.job_goal.pose.position.y = 6 # 12.7
job.job_goal.pose.orientation.w = 1
job.job_time = 3
while True:
if self.pub.get_num_connections() > 0:
self.pub.publish(job)
break
time.sleep(0.01)
def rob_pub_id(self, msg):
while True:
if self.id_pub.get_num_connections() > 0:
self.id_pub.publish(msg)
break
if __name__ == '__main__':
rospy.init_node('job_rob_publisher', anonymous=True)
A = job_rob_pub()
try:
A.job()
for rob in range(2): # No. indicates the no. of robots
rob_id = Int16()
rob_id.data = rob + 1
A.rob_pub_id(rob_id)
time.sleep(0.1)
except rospy.ROSInterruptException:
pass
def local_cube_dropped_by(self, event):
self.swarmie_state[event.swarmie_id] = False
if self.cube_dropped_pub is not None and event.swarmie_id == self.own_swarmie_id:
self.cube_dropped_pub.publish(Int16(data=self.own_swarmie_id))
self._reeval_block_counts()
self._reeval_distances()
self._cmdvel_pub.publish(cmd_vel)
self._rlight_pub.publish(rlight)
time.sleep(5)
cmd_vel.angular.z = 0
cmd_vel.linear.x = self._spd
self._cmdvel_pub.publish(cmd_vel)
self._rlight_pub.publish(rlight)
else:
cmd_vel.angular.z = self._steeringline * 3.1415 / 180.0
time1 = time.time()
elapsed_time = time1 - self._start
self._start = time1
self._accdist = self._accdist + c_speed_msg.data * elapsed_time;
rlight = Int16();
if self._accdist > self._distance:
cmd_vel.linear.x = 0.0
rlight.data = 1
else:
cmd_vel.linear.x = self._spd
rlight.data = 2
if self._obj < 1.0:
cmd_vel.linear.x = 0.0
rlight.data = 2
if self._c_estop == True:
cmd_vel.linear.x = 0.0
rlight.data = 1
self._cmdvel_pub.publish(cmd_vel)
self._rlight_pub.publish(rlight)
accdist = Float32();
def _endscenario(self):
msg = Int16(DroneState.LANDING.value)
self.main_drone_pub.publish(msg)
time.sleep(5)
self._state = ProgramState.INACTIVE
def _setManualOverride(self, msg):
if self.manual_override == False and msg.data == True:
self._log("Manual Override Requested")
self.manual_override = True
msg = Int16(DroneState.MANUAL.value)
self.main_drone_pub.publish(msg)
def odometry_callback(self, data):
self.time_new = rospy.Time.now()
x = data.pose.pose.position.x
y = data.pose.pose.position.y
orientation_q = data.pose.pose.orientation
orientation_list = [
orientation_q.x, orientation_q.y, orientation_q.z, orientation_q.w
]
(roll, pitch, yaw) = euler_from_quaternion(orientation_list)
self.heading_pub.publish(Float32(yaw))
x_index = np.int(x * self.resolution)
y_index = np.int(y * self.resolution)
if (x_index < 0):
x_index = 0
if (x_index > ((self.map_size_x / self.resolution) - 1)):
x_index = (self.map_size_x / self.resolution) - 1
if (y_index < 0):
y_index = 0
if (y_index > ((self.map_size_y / self.resolution) - 1)):
y_index = (self.map_size_y / self.resolution) - 1
x3, y3 = self.matrix[x_index, y_index, :]
self.desired_yaw_pub.publish(Float32(np.arctan2(y3, x3)))
f_x = np.cos(yaw) * x3 + np.sin(yaw) * y3
f_y = -np.sin(yaw) * x3 + np.cos(yaw) * y3
# calculate error
self.error = np.arctan(f_y / (2.5 * f_x))
if (self.error > np.pi):
self.error = self.error - 2 * np.pi
if (self.error < -np.pi):
self.error = self.error + 2 * np.pi
self.error_pub.publish(Float32(self.error))
time = self.time_new - self.time_old
dt = float(time.secs + (time.nsecs / 1000000000.0))
if (self.init_time and dt > 0.01):
# limit error history, calculate integral control
self.error_queue.append(self.error * dt)
self.integral = 0
for e in self.error_queue:
self.integral += e
if (self.integral > KI_UPPER_LIMIT):
self.integral = KI_UPPER_LIMIT
elif (self.integral < KI_LOWER_LIMIT):
self.integral = KI_LOWER_LIMIT
# correct sudden huge errors, calculate differential control
#~ self.derivative = (yaw - self.last_yaw)
self.derivative = (self.error - self.previous_error)
if (abs(self.derivative) > 0.001):
if (self.derivative > np.pi):
self.derivative = self.derivative - 2 * np.pi
if (self.error < -np.pi):
self.derivative = self.derivative + 2 * np.pi
self.derivative = self.derivative / dt
control = self.Kp * self.error + self.Ki * self.integral + self.Kd * self.derivative
else:
control = self.Kp * self.error + self.Ki * self.integral
self.previous_error = self.error
else:
control = self.Kp * self.error
# When the f_x_car_coordinate is negative, the car should move backwards
if (f_x > 0):
speed = SPEED
if (abs(self.error) <= (np.pi / 16)):
speed = speed * 1.15
if (abs(self.integral) <= (np.pi / 16)):
speed = speed * 1.15
if (abs(self.derivative) <= (np.pi / 16)):
speed = speed * 1.15
if speed > MAX_SPEED:
speed = MAX_SPEED
else:
speed = int(-SPEED / 2)
if (f_y > 0):
control = -np.pi / 2
if (f_y < 0):
control = np.pi / 2
steering = control * YAW_TO_STEERING_FACTOR + ANGLE_STRAIGHT
if (steering >= MAX_ANGLE_LEFT):
steering = UInt8(int(MAX_ANGLE_LEFT))
elif (steering <= MAX_ANGLE_RIGHT):
steering = UInt8(int(MAX_ANGLE_RIGHT))
else:
steering = UInt8(int(steering))
# turning circle data for visualization in rviz
#~ deg_steering = float(steering.data - 90)
#~ self.turning_circle = 2.0 * (WHEELBASE / math.sin(math.radians(deg_steering)))
#~ marker = Marker()
#~ marker.header.frame_id = "map"
#~ marker.type = Marker.CYLINDER
#~ marker.action = Marker.ADD
#~ marker.pose.position.x = x;
#~ marker.pose.position.y = y;
#~ marker.pose.position.z = 0;
#~ marker.pose.orientation.x = orientation_q.x;
#~ marker.pose.orientation.y = orientation_q.y;
#~ marker.pose.orientation.z = orientation_q.z;
#~ marker.pose.orientation.w = orientation_q.w;
#~ marker.scale.x = self.turning_circle;
#~ marker.scale.y = self.turning_circle;
#~ marker.scale.z = 0.1;
#~ marker.color.a = 0.5;
#~ marker.color.r = 1.0;
#~ marker.color.g = 0.1;
#~ marker.color.b = 0.1;
#~ self.truning_circle_pub.publish(marker)
self.last_yaw = yaw
self.time_old = self.time_new
self.init_time = 1
if not self.shutdown_:
self.steering_pub.publish(steering)
self.speed_pub.publish(Int16(speed))
target_location = [
pt.point.x, pt.point.y,
float(poster_locations[targets[curr_target]][2])
]
pos_saver = []
zone_entered = -1
while True:
# Phase changes
if phase == "Cue Up":
if mini_timer.getTime() > cue_duration:
phase = "Movement"
zone_entered = -1
message = Int16()
message.data = encoding_dict[phase] + targets[curr_target] + 1
publisher.publish(message)
master_log.append([
encoding_dict[phase] + targets[curr_target] + 1,
master_timer.getTime()
])
mini_timer = core.MonotonicClock()
if phase == "Movement":
if mini_timer.getTime() > move_duration:
phase = "Penalty"
message = Int16()
message.data = encoding_dict[phase] + targets[curr_target] + 1
publisher.publish(message)
master_log.append([
def shutdown(self):
print("shutdown!")
self.shutdown_ = True
self.speed_pub.publish(Int16(0))
rospy.sleep(1)
def forwardDrive():
#print("Para frente")
forwardRight.value = 1.0
reverseRight.value = 0.0
def reverseDrive():
#print("Girar para esquerda")
forwardRight.value = 0.0
reverseRight.value = 1.0
#Funcao da distancia com o ultrassom
rospy.init_node('encoderDireito', anonymous=True)
msg = Int16()
pub = rospy.Publisher('rwheel', Int16, queue_size=1)
pub2 = rospy.Publisher('girosr', Float32, queue_size=1)
encoder1 = DigitalInputDevice(21)
cont1 = 0
def parafrente():
forwardDrive()
inicio = cont1
start = time.time()
#giros = Float32()
while time.time() < start + 1:
news = time.time()
while (encoder1.value == 0):
pub.publish(msg)
def __init__(self):
self.zumy = Zumy()
# self.PID = PID()
# self.PID_another = PID_another()
# self.l_enc_v = l_encTospeed()
rospy.init_node('zumy_ros')
#self.cmd = (0,0)
#self.rate = rospy.Rate(50.0)
rospy.Subscriber('/cmd_vel1', Twist, self.cmd_callback,queue_size=1)
# rospy.Subscriber('/vel',Float64,self.updata_vel,queue_size = 1)
# rospy.Subscriber('/l_speed', Float64, self.l_enc_to_speed, queue_size = 10)
# rospy.Subscriber('/r_speed', Float64, self.r_enc_to_speed, queue_size = 10)
# rospy.Subscriber('/angular_return',Float64, self.kalman_callback, queue_size = 5)
self.lock = Condition()
# self.rate = rospy.Rate(1.0)
self.name = socket.gethostname()
# self.current_time = time.time()
global enc_data_r
global enc_data_l
# global a
# self.PID.clear()#set the arguments of PID
# self.PID.setKp(0.115)
# self.PID.setKi(0.0006)
# self.PID.setKd(0.0)
# self.PID.setSampleTime(0.0001)
# self.PID.setWindup(15)
# self.PID_another.clear()
# self.PID_another.setKp(0.45)
# self.PID_another.setKi(0.035)
# self.PID_another.setKd(0)
# self.PID_another.setSampleTime(0.001)
# self.PID_another.setWindup(15)
self.rate = rospy.Rate(70.0)
# self.heartBeat = rospy.Publisher('heartBeat', String, queue_size=5)
self.imu_pub = rospy.Publisher('Imu', Imu, queue_size = 1)
self.r_enc_pub = rospy.Publisher('/r_enc', Int16, queue_size = 1)
self.l_enc_pub = rospy.Publisher('/l_enc', Int16, queue_size = 1)
# self.ly_twist_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
# self.PID_pub = rospy.Publisher('/PID_gethey',Float64,queue_size=1)
self.kalman_pub = rospy.Publisher('/kalman_param', kalman, queue_size = 5)
self.nonekalman_linear = rospy.Publisher('/nonekalman_linear',Float64,queue_size=5)
self.nonekalman_angular = rospy.Publisher('/nonekalman_angular',Float64,queue_size=5)
self.enc_angular = rospy.Publisher('/enc_angular',Float64,queue_size=5)
# self.sudu_pub = rospy.Publisher('/vel',Float64,queue_size = 1)
self.linear_pub = rospy.Publisher('/linear_vel',Float64,queue_size = 1)
self.angular_pub = rospy.Publisher('/angular_vel',Float64,queue_size = 1)
self.imu_count = 0
self.a = 0
self.v = 0
self.enc_data_r = Int16()
self.enc_data_l = Int16()
self.enc_data_r.data = 0
self.enc_data_l.data = 0
self.hey = 0
# global enc_data_r
# global enc_data_l
# global a
# global radius
ly_date = [enc_data_r , enc_data_l]
global ly_date
self.enc_r_last = 0
self.enc_v_last = 0
self.radius = 3.81 / 2
# the robot`s wheel will make 1168 ticks/meter, and get the parament:1168*radius/100 = 0.212504
self.param = 0.213
#enc_data_r = 0
#enc_data_l = 0
# global enc_data_r
# global enc_data_l
numpy.save('ly_enc.npy',ly_date)
self.current_time = time.time()
self.last_time = self.current_time
def __init__(self):
RIGHT_CHANNEL = 'GPIO_PZ0'
LEFT_CHANNEL = 'LCD_BL_PW'
# Pull private parameters only for Wheel Encoder
channel = rospy.get_param('~channel')
period = rospy.get_param('~period')
# boolean for checking if the wheel encoder is for right/left wheel
is_right = channel == RIGHT_CHANNEL
# initialize GPIO bus mode
if GPIO.getmode() != GPIO.TEGRA_SOC:
GPIO.setmode(GPIO.TEGRA_SOC)
# Set publisher to publish on wheel encoder channel
topic = 'rwheel'
if channel == LEFT_CHANNEL:
topic = 'lwheel'
pub1 = rospy.Publisher(topic, Int16, queue_size=5)
pub2 = rospy.Publisher(channel, wheel_encoder_data, queue_size=5)
GPIO.setup(channel, GPIO.IN)
deltaT = 0.0
current_time = rospy.get_time()
previous_time = current_time
he_input = GPIO.input(channel)
tick_count = 0
total_tick_count = 0
rospy.loginfo(rospy.get_caller_id() + ' began wheel encoder ' +
channel)
while not rospy.is_shutdown():
current_time = rospy.get_time()
new_input = GPIO.input(channel)
if he_input != new_input:
he_input = new_input
tick_count += 1
total_tick_count += 1
deltaT += current_time - previous_time
if deltaT >= period:
msg1 = Int16()
msg1.data = total_tick_count
pub1.publish(msg1)
msg2 = wheel_encoder_data()
msg2.is_right = is_right
msg2.total_tick_count = total_tick_count
msg2.tick_count = tick_count
msg2.delta_t = deltaT
msg2.stamp.secs = current_time
msg2.is_ready = True
pub2.publish(msg2)
deltaT = 0
tick_count = 0
previous_time = current_time
GPIO.cleanup()
def callback(data):
sixt = Int16()
sixt.data = data.num1 + data.num2
rospy.loginfo(sixt)
pub = rospy.Publisher('/sum', Int16, queue_size=10)
pub.publish(sixt)
def tail(self):
self._tail += 50
if self._tail > 255: self._tail = 0
print("Tail brightness", self._tail)
self.pub_tail.publish(Int16(self._tail))
TEST_IMAGE_PATHS = [
os.path.join(PATH_TO_TEST_IMAGES_DIR, 'image{}.jpg'.format(i))
for i in range(1, 3)
]
# Size, in inches, of the output images.
IMAGE_SIZE = (12, 8)
##-----------------------------------------------------------------------------------------------##
##-----------------------------------------------------------------------------------------------##
#--------------------- ROS NODE CONFIGURATION -------------------------------
rospy.init_node('object_detector', anonymous=False)
# Set up Publishers
x_loc = Int16()
x_loc.data = 0
xloc_publisher = rospy.Publisher('/xaxis_location', Int16, queue_size=1)
# Set up Subscribers
def image_callback(image_msg):
global rgbImg
# Convert ros image to cv image
rgbImg = bridge.imgmsg_to_cv2(image_msg, "bgr8")
image_subscriber = rospy.Subscriber('/camera/rgb/image_color', Image,
image_callback)
def sethead(self):
print("Set heading to", self._direct,
"; also resets tail and stablize")
self.pub_heading.publish(Int16(self._direct))
self._stab = True
self._tail = 0 #resets both
def main(args=None):
global velocity
global cruise_mode
global cruise_speed
rclpy.init()
node = rclpy.create_node('teleop_twist_keyboard')
accelPub = node.create_publisher(Int16, '/dbw_cmd/Accel', qos_profile_default)
brakePub = node.create_publisher(Int16, '/dbw_cmd/Brake', qos_profile_default)
steerPub = node.create_publisher(Int16, '/dbw_cmd/Steer', qos_profile_default)
thread_velo = threading.Thread(target=execute, args=(node,))
thread_velo.start()
thread_cruise = threading.Thread(target=cruise, args=(accelPub, brakePub,))
thread_cruise.start()
handle_set = 0
accelACT = 650
brakeACT = 8500
status = 0
accelACT_MAX = 3000
brakeACT_MAX = 27000
handle_set_MAX = 440
propulsion_rate = ((accelACT/accelACT_MAX)-(brakeACT/brakeACT_MAX))/(2)*100
try:
print(msg)
print(vels(propulsion_rate,handle_set, accelACT, brakeACT))
while(1):
accelACT = accelACT if accelACT <= accelACT_MAX else accelACT_MAX
brakeACT = brakeACT if brakeACT <= brakeACT_MAX else brakeACT_MAX
if abs(handle_set) >= handle_set_MAX:
Hsigned = -1 if handle_set < 0 else 1
handle_set = Hsigned * handle_set_MAX
propulsion_rate = ((accelACT/accelACT_MAX)-(brakeACT/brakeACT_MAX))/(1)*100
#print('='*30,"\n")
key = getKey()
#print('\n\n\n\n\n','='*30, sep='')
if key in cruiseControl.keys():
if key == "k" :
cruise_mode = cruiseControl[key]
if key == "l" :
cruise_mode = cruiseControl[key]
if key == "i" :
cruise_speed += cruiseControl[key]
if key == "m" :
cruise_speed += cruiseControl[key]
elif key in moveBindings.keys():
if key == "q" :
handle_set = 0
handle_set += moveBindings[key]
elif key in speedBindings.keys():
if key == "w" :
brakeACT = 0
accelACT += speedBindings[key]
if key == "s" :
accelACT = 650
brakeACT += speedBindings[key]
if key == "x" :
accelACT = 0
brakeACT += speedBindings[key]
if (status == 14):
print(msg)
status = (status + 1) % 15
else:
print("BRACE!! EMERGENCY STOP!!!\n"*3)
###MUST be ON THREAD!!!!
os.system('''for k in {1..3};
do
play -nq -t alsa synth 0.2 sine 544;
sleep 0.03;
play -nq -t alsa synth 0.2 sine 544;
sleep 0.03;
play -nq -t alsa synth 0.2 sine 544;
sleep 0.03;
play -nq -t alsa synth 0.2 sine 544;
sleep 0.03;
done &''')
brakeACT = brakeACT_MAX
if (key == '\x03'):
break
print("cruise_mode = ", cruise_mode , " , cruise_speed = ", cruise_speed)
print(vels(propulsion_rate,handle_set, accelACT, brakeACT))
accel = Int16()
brake = Int16()
steer = Int16()
accel.data = accelACT
brake.data = brakeACT
steer.data = handle_set
if cruise_mode == 0:
brakePub.publish(brake)
accelPub.publish(accel)
steerPub.publish(steer)
except Exception as e:
print(e)
finally:
accel = Int16()
brake = Int16()
steer = Int16()
accel.data = 0
brake.data = 8500
steer = 0
brakePub.publish(brake)
accelPub.publish(accel)
steerPub.publish(steer)
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def reset(self):
print("Reset heading")
self.pub_reset.publish(Int16(0)) # value ignored
self._direct = self._speed = 0
#!/usr/bin/env python
import rospy
from std_msgs.msg import Int16
from assignment0.msg import TwoInt
result = Int16()
def callback(msg):
result.data = msg.num1 + msg.num2
rospy.loginfo(result)
def adder():
rospy.init_node('adder')
pub = rospy.Publisher("/sum", Int16, queue_size=10)
sub = rospy.Subscriber("/numbers", TwoInt, callback)
rate = rospy.Rate(10)
while not rospy.is_shutdown():
pub.publish(result)
rate.sleep()
if __name__ == '__main__':
try:
adder()
except rospy.ROSInterruptException:
pass
def callback(data):
pub_pan = rospy.Publisher('pan_controller/command', Float64, queue_size=10)
pub_tilt = rospy.Publisher('tilt_controller/command',
Float64,
queue_size=10)
#pub_proj = rospy.Publisher('proj_array', Int16MultiArray, queue_size=10)
pub_int = rospy.Publisher('finish_pantilt', Int16, queue_size=10)
pan_speed = data.speed.x
tilt_speed = data.speed.y
set_pan_speed = rospy.ServiceProxy('/pan_controller/set_speed', SetSpeed)
set_pan_speed(pan_speed)
set_tilt_speed = rospy.ServiceProxy('/tilt_controller/set_speed', SetSpeed)
set_tilt_speed(tilt_speed)
x_point = data.position.x
y_point = data.position.y
z_point = data.position.z
rad_pan = math.atan2(y_point, x_point)
#print math.degrees(rad_pan)
distance = math.sqrt(x_point * x_point + y_point * y_point)
#print distance
ud_z_point = 1.21
rad_tilt = math.atan2(ud_z_point, distance)
#print math.degrees(rad_tilt)
float_pan = Float64()
float_tilt = Float64()
float_pan = rad_pan - 1.5708
float_pan = checkPanRadian(float_pan)
target_pan = float_pan
float_tilt = rad_tilt
float_tilt = checkTiltRadian(float_tilt)
target_tilt = float_tilt
pub_pan.publish(float_pan)
pub_tilt.publish(float_tilt)
offset_tilt = 0.04 #2.5 degree
offset_pan = 0.04
while True:
#print "pass"
current_tilt = get_tilt_position()
current_pan = get_pan_position()
#print "c", current_pan, current_tilt
#print "t", target_pan, target_tilt
if current_tilt < target_tilt + offset_tilt and current_tilt > target_tilt - offset_tilt and current_pan < target_pan + offset_pan and current_pan > target_pan - offset_pan:
break
rospy.set_param('control_pantilt/current_tilt_degree', current_tilt)
rospy.set_param('control_pantilt/current_pan_degree', current_pan)
rospy.set_param('control_pantilt/current_x_position', x_point)
rospy.set_param('control_pantilt/current_y_position', y_point)
#proj = Int16MultiArray()
#proj_array = [0,1]
#proj.data = proj_array
#pub_proj.publish(proj)
fin_message = Int16()
fin_message = 1
pub_int.publish(fin_message)
print("finish pantilt")
def shutdown(self):
# set speed to 0
print("shutdown!")
self.shutdown_ = True
self.vel_pub.publish(Int16(0))
rospy.sleep(1)
def callback(data): #print (data.a, data.b) pub.publish(Int16(data.a + data.b))
