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))