def lidar_callback(data): global prev_threshold_angle, integral_prior gauss_range = np.arange(0,1080,1) gaussian_weights = gaussian(gauss_range,540,150) gaussian_weights = gaussian_weights + (1 - gaussian_weights[0]) #limited_ranges = limited_ranges*gaussian_weights limited_ranges = np.asarray(data.ranges) limited_ranges[0:right_extreme] = 0.0 limited_ranges[(left_extreme + 1):] = 0.0 limited_ranges[(left_extreme + 1)] = limited_ranges[left_extreme] indices = np.where(limited_ranges >= lidar_max_range)[0] limited_ranges[indices] = lidar_max_range - 0.1 #data.range_m - 0.1 disparities = find_disparities(limited_ranges, disparity_threshold) # Experimental section right=[] left=[] front=[] # Handling Multiple Disparities a = 14 for i in disparities: if i>540-a and i<540+a: front.append(i) elif i<540: right.append(i) #print(right) else: left.append(i) if len(right)>len(left) and len(right)>len(front): disparities=right elif len(left)>len(right) and len(left)>len(front): disparities=left else: disparities=front new_ranges = extend_disparities(limited_ranges, disparities, car_width) new_ranges = new_ranges*gaussian_weights max_value = max(new_ranges) target_distances = np.where(new_ranges >= (max_value - 1))[0] #index values driving_distance_index, chosen_index = calculate_target_distance(target_distances) driving_angle = calculate_angle(driving_distance_index) thresholded_angle = threshold_angle(driving_angle) behind_car = np.asarray(data.ranges) behind_car_right = behind_car[0:right_extreme] behind_car_left = behind_car[(left_extreme+1):] #change the steering angle based on whether we are safe thresholded_angle = adjust_turning_for_safety(behind_car_left, behind_car_right, thresholded_angle) velocity = calculate_min_turning_radius(thresholded_angle) velocity = threshold_speed(velocity, new_ranges[driving_distance_index], new_ranges[540]) # Publish drive and Steer msg = AckermannDriveStamped() Kd = 0.02 #0.055 Ki = 0.075 Kp = 0.75 integral = integral_prior + (thresholded_angle * 0.004) msg.drive.steering_angle = Kp*thresholded_angle + (Kd*(thresholded_angle - prev_threshold_angle)/0.004) + Ki*integral prev_threshold_angle = thresholded_angle integral_prior = integral max_possible_distance = data.ranges[chosen_index] delta_l = data.ranges[driving_distance_index] - lidar_max_range delta_l_low = 7#7 kp_vel = 1 / (1 + np.exp(-delta_l)) kp_vel = np.clip(kp_vel,0.2,0.5)#0.5 print('delta l', delta_l) print('kp_vel',kp_vel) if delta_l > delta_l_low: velocity = kp_vel*absolute_max_speed Kd = 0.02#0.055 #0.02 Ki = 0.08#uh #0.08 Kp = 1.1#kjkj #1.1 integral = integral_prior + (thresholded_angle * 0.004) msg.drive.steering_angle = Kp*thresholded_angle + (Kd*(thresholded_angle - prev_threshold_angle)/0.004) + Ki*integral prev_threshold_angle = thresholded_angle integral_prior = integral print('velocity', velocity) msg.drive.speed = velocity pub_drive_param.publish(msg) current_time = rospy.Time.now() # Publish Modified Scan scan = LaserScan() scan.header.stamp = current_time scan.header.frame_id = 'ego_racecar/laser' scan.angle_min = -1.57 scan.angle_max = 1.57 scan.angle_increment = 3.14 / 1080 scan.time_increment = (1.0 / 250) / (1080) scan.range_min = 0.0 scan.range_max = 270.0 scan.ranges = new_ranges scan.intensities = [] scan_pub.publish(scan) # Publish marker markermsg = Marker() markermsg.header.frame_id = "ego_racecar/laser" markermsg.header.stamp = current_time markermsg.id = 0 markermsg.type = 2 markermsg.action = 0 markermsg.pose.position.x = np.sin(math.pi-np.deg2rad(((driving_distance_index - 180)/4)))*new_ranges[driving_distance_index] markermsg.pose.position.y = np.cos(math.pi-np.deg2rad(((driving_distance_index - 180)/4)))*new_ranges[driving_distance_index] markermsg.pose.position.z = 0 markermsg.scale.x = 0.5 markermsg.scale.y = 0.5 markermsg.scale.z = 0.5 markermsg.pose.orientation.x = 0 markermsg.pose.orientation.y = 0 markermsg.pose.orientation.z = 0 markermsg.pose.orientation.w = 1.0 markermsg.color.r = 0.0 markermsg.color.g = 1.0 markermsg.color.b = 0.0 markermsg.color.a = 1.0 markermsg.lifetime = rospy.Duration() marker_pub.publish(markermsg) """Scale the speed in accordance to the forward distance"""
from std_srvs.srv import Empty from tf.transformations import euler_from_quaternion import tensorflow as tf import numpy as np import copy import matplotlib.pyplot as plt import subprocess import time home_dir = "/home/arun/" base_dir = "/home/arun/ROS/catkin_ws/src/cadrl" # Sensor data stored in a global variable so it can be accessed asynchronously sensor_data = LaserScan().ranges odom_data = Odometry().twist.twist is_crashed = False # Collision frequencies for plotting are also global variables so we can # access them even after the main program is shut down iterations = [] collision_frequencies = [] cumulative_reward = [] ######### Initialize Q-Network ################ # parameters learning_rate = 0.001 n_hidden_1 = 100 n_hidden_2 = 300
def cbLaser(self, msg=LaserScan()): self.point_set = msg.ranges self.max_range = msg.range_max self.min_range = msg.range_min
def publish_distance_data(self, stamp): dists = [OUT_OF_RANGE] * NB_INFRARED_SENSORS dist_tof = OUT_OF_RANGE # Calculate distances for i, key in enumerate(self.distance_sensors): dists[i] = interpolate_table( self.distance_sensors[key].getValue(), DISTANCE_TABLE) # Publish range: Infrared for i, key in enumerate(self.distance_sensors): msg = Range() msg.header.stamp = stamp msg.header.frame_id = key msg.field_of_view = self.distance_sensors[key].getAperture() msg.min_range = INFRARED_MIN_RANGE msg.max_range = INFRARED_MAX_RANGE msg.range = dists[i] msg.radiation_type = Range.INFRARED self.distance_sensor_publishers[key].publish(msg) # Publish range: ToF if self.tof_sensor: dist_tof = interpolate_table(self.tof_sensor.getValue(), TOF_TABLE) msg = Range() msg.header.stamp = stamp msg.header.frame_id = 'tof' msg.field_of_view = self.tof_sensor.getAperture() msg.min_range = TOF_MAX_RANGE msg.max_range = TOF_MIN_RANGE msg.range = dist_tof msg.radiation_type = Range.INFRARED self.tof_publisher.publish(msg) # Max range of ToF sensor is 2m so we put it as maximum laser range. # Therefore, for all invalid ranges we put 0 so it get deleted by rviz msg = LaserScan() msg.header.frame_id = 'laser_scanner' msg.header.stamp = stamp msg.angle_min = - 150 * pi / 180 msg.angle_max = 150 * pi / 180 msg.angle_increment = 15 * pi / 180 msg.range_min = SENSOR_DIST_FROM_CENTER + INFRARED_MIN_RANGE msg.range_max = SENSOR_DIST_FROM_CENTER + INFRARED_MAX_RANGE msg.ranges = [ dists[3] + SENSOR_DIST_FROM_CENTER, # -150 OUT_OF_RANGE, # -135 OUT_OF_RANGE, # -120 OUT_OF_RANGE, # -105 dists[2] + SENSOR_DIST_FROM_CENTER, # -90 OUT_OF_RANGE, # -75 OUT_OF_RANGE, # -60 dists[1] + SENSOR_DIST_FROM_CENTER, # -45 OUT_OF_RANGE, # -30 dists[0] + SENSOR_DIST_FROM_CENTER, # -15 OUT_OF_RANGE, # 0 dists[7] + SENSOR_DIST_FROM_CENTER, # 15 OUT_OF_RANGE, # 30 dists[6] + SENSOR_DIST_FROM_CENTER, # 45 OUT_OF_RANGE, # 60 OUT_OF_RANGE, # 75 dists[5] + SENSOR_DIST_FROM_CENTER, # 90 OUT_OF_RANGE, # 105 OUT_OF_RANGE, # 120 OUT_OF_RANGE, # 135 dists[4] + SENSOR_DIST_FROM_CENTER, # 150 ] self.laser_publisher.publish(msg)
def convert_to_scan(ppl, header): global tf_listener # ppl 1024 points in 16 channel, so scan should have floor(1024/16) camera_translation_base = [0, 0, 0] base_frame = 'SimpleFlight' camera_frame = 'SimpleFlight/odom_local_ned' #camera_frame='SimpleFlight/LidarCustom' if tf_listener.frameExists(base_frame) and tf_listener.frameExists( camera_frame): # Get transformation try: time = tf_listener.getLatestCommonTime(base_frame, camera_frame) camera_translation_base, camera_orientation_base = tf_listener.lookupTransform( base_frame, camera_frame, time) except (tf.Exception, tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print('tf lookuptransform fail ', base_frame, camera_frame) return print(camera_frame, ' pose ', camera_translation_base) scanmsg = LaserScan() # scanmsg.header.frame_id='SimpleFlight' # scanmsg.header.frame_id='odom_frame' scanmsg.header.frame_id = 'SimpleFlight/odom_local_ned' #scanmsg.header.frame_id='SimpleFlight/LidarCustom' #scanmsg.header.frame_id='front_left_custom_optical/static' #scanmsg.header.frame_id='front_left_custom_optical' scanmsg.header.stamp = header.stamp #scanmsg.header.stamp= rospy.get_rostime() #scanmsg.header.seq=header.seq scanmsg.range_min = 0 scanmsg.range_max = 50 scansize = int(len(ppl) / 16) # the ppl are in SimpleFlight frame, need to convert it to the lidar frame for proper display in rviz. after conversion, the scan data point only means the xy location of lidar point. # for now it is sqrt(x^2+y^2) of the lidar reflection. could be ground, could be from an obstacle. there is no telling. will change it later so if its ground point then change it to max range. note that the mapping process will ignore the max range lidar points when building map. ppl_lidarframe = np.array(list(ppl)) - camera_translation_base #ppl_lidarframe = np.array(list(ppl)) outrim = ppl_lidarframe[0:64] #calculate the angle of the first point if ppl_lidarframe[0, 0] == 0: angle_start = 3.14 / 2 else: angle_start = np.math.atan(ppl_lidarframe[0, 1] / ppl_lidarframe[0, 0]) if ppl_lidarframe[0, 0] < 0 and ppl_lidarframe[0, 1] > 0: angle_start = angle_start + 3.14 if ppl_lidarframe[0, 0] < 0 and ppl_lidarframe[0, 1] < 0: angle_start = angle_start + 3.14 if ppl_lidarframe[0, 0] > 0 and ppl_lidarframe[0, 1] < 0: angle_start = angle_start + 3.14 * 2 print(' start angle ', angle_start * 360 / 6.28, ppl_lidarframe[0, 0], ppl_lidarframe[0, 1], ppl_lidarframe[1, 0], ppl_lidarframe[1, 1]) #ppl_angles(ppl_lidarframe[0:64]) #debugging, print the angle of the 64 points #scanmsg.angle_min = -3.14 scanmsg.angle_min = angle_start scanmsg.angle_max = angle_start + 3.14 * 2 #scanmsg.angle_max = 3.14 scanmsg.angle_increment = (scanmsg.angle_max - scanmsg.angle_min) / scansize #outrimlist=[] #for pt in outrim: # outrimlist.append([pt.x, pt.y]) print('pt1 ', type(outrim[0]), outrim.shape) newrim = ppl_lidarframe[0:64] for i in range(1, 1): newrim = ppl_lidarframe[i * 64:(i + 1) * 64] for j in range(len(newrim)): if newrim[j][2] < -0.3: outrim[j] = newrim[j] #print('replace') max_x = ppl_max_x(ppl[0:64]) i = 10 scanmsg.ranges = np.linalg.norm(ppl_lidarframe[i * 64:(i + 1) * 64, 0:2], axis=1) #print(scanmsg.ranges) #scanmsg.ranges=[max_x]*64 #scanmsg.ranges=np.linalg.norm(newrim, axis=1) #scanmsg.ranges=np.linalg.norm(outrim, axis=1) return scanmsg
msg.pose.pose.orientation.y = q[1] msg.pose.pose.orientation.z = q[2] msg.pose.pose.orientation.w = q[3] return msg with open('intel.clf') as dataset: with rosbag.Bag('aces.bag', 'w') as bag: for line in dataset.readlines(): line = line.strip() tokens = line.split(' ') if len(tokens) <= 2: continue if tokens[0] == 'FLASER': msg = LaserScan() num_scans = int(tokens[1]) if num_scans != 180 or len(tokens) < num_scans + 9: rospy.logwarn("unsupported scan format") continue msg.header.frame_id = 'laser' t = rospy.Time(float(tokens[(num_scans + 8)])) msg.header.stamp = t msg.angle_min = -90.0 / 180.0 * pi msg.angle_max = 90.0 / 180.0 * pi msg.angle_increment = pi / (num_scans - 1.0) msg.time_increment = 0.2 / 360.0 msg.scan_time = 0.2 msg.range_min = 0.001
def __init__(self, bot_name): # bot name robot_name = rospy.get_param('~robot_name') # red_bot or blue_bot self.name = robot_name print("self.name", self.name) # velocity publisher self.vel_pub = rospy.Publisher('cmd_vel', Twist, queue_size=1) # navigation publisher self.client = actionlib.SimpleActionClient('move_base', MoveBaseAction) # Lidar self.scan = LaserScan() topicname_scan = "/" + self.name + "/scan" self.lidar_sub = rospy.Subscriber(topicname_scan, LaserScan, self.lidarCallback) self.front_distance = DISTANCE_TO_ENEMY_INIT_VAL # init self.front_scan = DISTANCE_TO_ENEMY_INIT_VAL # usb camera self.img = None self.camera_preview = True self.bridge = CvBridge() topicname_image_raw = "/" + self.name + "/image_raw" self.image_sub = rospy.Subscriber(topicname_image_raw, Image, self.imageCallback) self.red_angle = COLOR_TARGET_ANGLE_INIT_VAL # init self.blue_angle = COLOR_TARGET_ANGLE_INIT_VAL # init self.green_angle = COLOR_TARGET_ANGLE_INIT_VAL # init self.red_distance = DISTANCE_TO_ENEMY_INIT_VAL # init # FIND_ENEMY status self.find_enemy = FIND_ENEMY_SEARCH self.find_wait = 0 self.enemy_direct = 1 # joint self.joint = JointState() self.joint_sub = rospy.Subscriber('joint_states', JointState, self.jointstateCallback) self.wheel_rot_r = 0 self.wheel_rot_l = 0 self.moving = False # twist self.x = 0 self.th = 0 # war status topicname_war_state = "/" + self.name + "/war_state" self.war_state = rospy.Subscriber(topicname_war_state, String, self.stateCallback) self.my_score = 0 self.enemy_score = 0 self.act_mode = ActMode.SEARCH #BASIC # odom topicname_odom = "/" + self.name + "/odom" self.odom = rospy.Subscriber(topicname_odom, Odometry, self.odomCallback) # amcl pose topicname_amcl_pose = "/" + self.name + "/amcl_pose" self.amcl_pose = rospy.Subscriber(topicname_amcl_pose, PoseWithCovarianceStamped, self.AmclPoseCallback) # time self.time_start = time.time()
return mk rospy.loginfo('update') mks = MarkerArray() for i, radar in enumerate(r): ns = str(int(radar[1])) id = str(int(radar[2])) v = radar[6] p = radar[3:6] mk = make_marker(p, ns, 2 * i + 1, id, c=v) tk = make_marker(p, ns, 2 * i, id, c=v, text=True) mks.markers.append(mk) # mks.markers.append(tk) pub.publish(mks) ls = LaserScan() ls.header.frame_id = 'map' ls.header.stamp = rospy.Time.now() ls.range_max = 100.0 ls.range_min = 0 ls.angle_max = 2.094395 ls.angle_min = -2.094395 ls.angle_increment = 0.017453 ls.ranges = l.tolist() publ.publish(ls) rospy.spin()
def _publish_scan_message(self, center_x, center_y, timestamp, contours_image): if self.prev_scan_time is None: self.prev_scan_time = datetime.datetime.now() return if PROFILE_SCAN_GENERATOR: ts = time.time() if self.scans_pickle_path is None: scan_ranges = contours_scan.generate( contours_image, center_x=center_x, center_y=center_y, min_angle=self.min_angle, max_angle=self.max_angle, samples_num=self.samples_num, min_distance=self.min_distance_pixels, max_distance=self.max_distance_pixels, resolution=self.resolution, r_primary_search_samples=self.r_primary_search_samples, r_secondary_search_step=self.r_secondary_search_step) else: scan_ranges = self.timestamp_to_scan[timestamp] if self.noise_sigma != 0: noise = np.random.normal(loc=0, scale=self.noise_sigma, size=len(scan_ranges)) scan_ranges = scan_ranges + noise curr_scan_time = datetime.datetime.now() if PROFILE_SCAN_GENERATOR: te = time.time() delta = (te - ts) if self.scan_idx == 0: self.mean_scan_time = delta else: self.mean_scan_time = float(self.mean_scan_time) * ( self.scan_idx - 1) / self.scan_idx + delta / self.scan_idx rospy.loginfo( '%s :: Synthetic scan generation time: %f[sec], mean: %f[sec]' % (self.namespace, delta, self.mean_scan_time)) if TRACK_NAN_IN_SCANS: if np.isnan(scan_ranges).any(): self.any_nan += 1 if np.isnan(scan_ranges).all(): self.all_nans += 1 rospy.loginfo('%s :: Any NaN occurrences: %d' % (self.namespace, self.any_nan)) rospy.loginfo('%s :: All NaN occurrences: %d' % (self.namespace, self.all_nans)) if TRACK_INF_IN_SCANS: inf_rate = float(np.sum(np.isinf(scan_ranges))) / len(scan_ranges) if self.scan_idx == 0: self.mean_inf_rate = inf_rate else: self.mean_inf_rate = float(self.mean_inf_rate) * ( self.scan_idx - 1) / self.scan_idx + inf_rate / self.scan_idx rospy.loginfo('%s :: Mean inf rate: %f' % (self.namespace, self.mean_inf_rate)) self.scan_idx += 1 laser_scan = LaserScan() laser_scan.header.stamp = rospy.rostime.Time.now() laser_scan.header.frame_id = self.frame_id laser_scan.angle_min = self.min_angle laser_scan.angle_max = self.max_angle laser_scan.angle_increment = (self.max_angle - self.min_angle) / self.samples_num laser_scan.scan_time = (curr_scan_time - self.prev_scan_time).seconds laser_scan.range_min = self.min_distance_pixels * self.resolution laser_scan.range_max = self.max_distance_pixels * self.resolution laser_scan.ranges = scan_ranges self.scan_pub.publish(laser_scan) self.prev_scan_time = curr_scan_time
self.buf_len = buf_len self.average = 0 def update_average(self, val): if len(self.buf) >= self.buf_len: for i in range(self.buf_len - 1): self.buf[i] = self.buf[i+1] self.buf[self.buf_len - 1] = val else: self.buf.append(val) self.average = mean(self.buf) return self.average scan_data = LaserScan() new_scan_flag = 0 # ---------------------------------------------- def update_scan(data): global scan_data global new_scan_flag new_scan_flag = 1 scan_data = data #----------------------------------------- def main_collision_avoidance(): global scan_data
# load vae.load_json(vae_model_path) # create training dataset dataset = archive_to_lidar_dataset("~/navrep/datasets/V/ian", limit=180) if len(dataset) == 0: raise ValueError("no scans found, exiting") print(len(dataset), "scans in dataset.") dataset = dataset[:500000:100] # split into batches: total_length = len(dataset) num_batches = int(np.floor(total_length / batch_size)) dummy_msg = LaserScan() dummy_msg.range_max = 100.0 dummy_msg.ranges = range(1080) ring_accuracy_per_example = np.ones((len(dataset), )) * -1 for idx in range(num_batches): batch = dataset[idx * batch_size:(idx + 1) * batch_size] scans = batch obs = np.clip(scans.astype(np.float) / MAX_LIDAR_DIST, 0.0, MAX_LIDAR_DIST) obs = obs.reshape(batch_size, 1080, 1) obs_pred = vae.encode_decode(obs) error = abs(obs - obs_pred) threshold = np.minimum(0.001, abs(obs * 0.05)) is_right = error <= threshold
#!/usr/bin/env python import rospy import math import tf import numpy import math from sensor_msgs.msg import LaserScan from nav_msgs.msg import MapMetaData from xunjian_nav.msg import GridLaser from xunjian_nav.msg import GridPoint map_meta = MapMetaData() laser_data = LaserScan() #offset from laser to base_link X_OFFSET = 0.21 Y_OFFSET = 0.0 #map callback, update map data,actually, map data need no update def map_callback(msg): global map_meta map_meta = msg #laser callback, update laser data def laser_callback(msg): global laser_data laser_data.angle_increment = msg.angle_increment laser_data.angle_max = msg.angle_max laser_data.angle_min = msg.angle_min laser_data.ranges = msg.ranges
import copy AnglesAll = np.arange(-2.0716333668668598, 0.486305713654, 0.00325437542051) AnglesPico = np.arange(-0.499194979668 - np.pi / 2, 0.534045875072 - np.pi / 2, 0.00463336659595) AnglesStruct = np.arange(-0.551840126514, 0.486305713654, 0.00325437542051) AnglesPicoAll = np.arange(-2.0716333668668598, -1.03674198314468, 0.00325437542051) IndPico = np.round(np.interp(AnglesPicoAll, AnglesAll, range(len(AnglesAll)))).astype(int) IndStruct = np.round(np.interp(AnglesStruct, AnglesAll, range(len(AnglesAll)))).astype(int) ScanCombine0 = LaserScan(angle_min=-2.0716333668668598, angle_max=0.486305713654, angle_increment=0.00325437542051, time_increment=0.0001, scan_time=0.0786, range_min=0.3, range_max=8) ScanCombine0.header.frame_id = 'camera_depth_frame' ScanCombine0.ranges = np.repeat(np.nan, len(AnglesAll)) Seq = 0 ScanCombine = copy.deepcopy(ScanCombine0) def InsertData(data, type): global ScanCombine, AnglesPicoAll, AnglesPico, IndPico, Seq, IndStruct if type == 0: ScanCombine.ranges[IndPico] = np.interp(AnglesPicoAll, AnglesPico, data.ranges)
#!/usr/bin/python3 import rospy from sensor_msgs.msg import NavSatFix from sensor_msgs.msg import LaserScan from sensor_msgs.msg import Imu from std_msgs.msg import String import numpy as np rospy.init_node("tester", anonymous=True) pub = rospy.Publisher("test_top", LaserScan, queue_size=10) ls_1_og_input = LaserScan() ls_1_og_input.ranges = np.zeros(1101).tolist() ls_2_og_input = LaserScan() ls_2_og_input.ranges = np.zeros(1101).tolist() def ls_callback(data): global ls_og_input, ls_1_og_input, ls_2_og_input if data.header.frame_id == 'cloud_POS_000_DIST1': ls_1_og_input = data join() if data.header.frame_id == 'cloud_POS_000_DIST2': ls_2_og_input = data join() def join(): global ls_og_input, ls_1_og_input, ls_2_og_input ls_og_input = LaserScan() ls_og_input = ls_1_og_input ls_og_input.header.frame_id = 'cloud'
def doAScan(direction): ######################################################## global moving scan_rate = 50 # hertz between steps angle_slack = 0.25 if direction: angle_start = 170 angle_stop = 10 angle_step = -1 else: angle_start = 10 angle_stop = 170 angle_step = 1 servo_pub.publish(angle_start) moving = False scan = LaserScan() num_readings = 0 scan_time = rospy.Time.now() r = rospy.Rate(scan_rate) ranges = [] for angle in range(angle_start, angle_stop, angle_step): if moving: break servo_pub.publish(angle) wait_start = rospy.Time.now() r.sleep() # while latest_std > std_threshold: # if rospy.Time.now() - wait_start > rospy.Duration(std_timeout): # rospy.loginfo("-W- range_to_laser timed out waiting for std_dev (%0.2f) to get below threshold (%0.2f)" % (latest_std, std_threshold) ) # break # rospy.loginfo("angle %d range:%0.2f" % (angle, latest_range)) num_readings += 1 ranges_ary = array(ranges[-4:]) # if ranges_ary.std() < 0.04: # scan.intensities.append(100) ranges.append((latest_range * scale) - 0.03) # else: # ranges.append(1e4) # scan.intensities.append(0) # if latest_range > 0: # scan.intensities.append( 1 / ( ranges_ary.std() * 10 + .1 ) ) if latest_range > 0: scan.intensities.append(1 / latest_range) else: scan.intensities.append(0) scan.angle_min = -1.57 scan.angle_max = 1.57 if direction: ranges.reverse() scan.intensities.reverse() scan.angle_min += angle_slack scan.angle_max += angle_slack scan.ranges = ranges scan.header.stamp = scan_time scan.header.frame_id = 'scanner' # TODO: change this to 'laser_frame'. (create the transform first) scan.angle_increment = 3.14 / num_readings scan.time_increment = (1 / scan_rate) scan.range_min = 0.01 * scale scan.range_max = 2.0 * scale if not moving: scan_pub.publish(scan)
def spin(self): encoders = [0, 0] self.x = 0 # position in xy plane self.y = 0 self.th = 0 then = rospy.Time.now() # things that don't ever change scan_link = rospy.get_param('~frame_id', 'base_laser_link') scan = LaserScan(header=rospy.Header(frame_id=scan_link)) scan.angle_min = 0 scan.angle_max = 6.26 scan.angle_increment = 0.017437326 scan.range_min = 0.020 scan.range_max = 5.0 odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_link') # main loop of driver r = rospy.Rate(5) self.robot.requestScan() while not rospy.is_shutdown(): # prepare laser scan scan.header.stamp = rospy.Time.now() #self.robot.requestScan() scan.ranges = self.robot.getScanRanges() # get motor encoder values left, right = self.robot.getMotors() # send updated movement commands self.robot.setMotors( self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]), abs(self.cmd_vel[1]))) # ask for the next scan while we finish processing stuff self.robot.requestScan() # now update position information dt = (scan.header.stamp - then).to_sec() then = scan.header.stamp d_left = (left - encoders[0]) / 1000.0 d_right = (right - encoders[1]) / 1000.0 encoders = [left, right] dx = (d_left + d_right) / 2 dth = (d_right - d_left) / (BASE_WIDTH / 1000.0) x = cos(dth) * dx y = -sin(dth) * dx self.x += cos(self.th) * x - sin(self.th) * y self.y += sin(self.th) * x + cos(self.th) * y self.th += dth # prepare tf from base_link to odom quaternion = Quaternion() quaternion.z = sin(self.th / 2.0) quaternion.w = cos(self.th / 2.0) # prepare odometry odom.header.stamp = rospy.Time.now() odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = dx / dt odom.twist.twist.angular.z = dth / dt # publish everything self.odomBroadcaster.sendTransform( (self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then, "base_link", "odom") self.scanPub.publish(scan) self.odomPub.publish(odom) # wait, then do it again r.sleep() # shut down self.robot.setLDS("off") self.robot.setTestMode("off")
else if front_rear == "rear_laser" geo_msg.transform.translation.x = -0.463 geo_msg.transform.translation.y = 0.001 geo_msg.transform.translation.z = 0.454 angles = tf.transformations.quaternion_from_euler(0,0,-3.14) geo_msg.transform.rotation.x = angles[0] geo_msg.transform.rotation.y = angles[1] geo_msg.transform.rotation.z = angles[2] geo_msg.transform.rotation.w = angles[3] tf_msg.transforms.append(geo_msg) odom_msg.append(tf_msg) scan = LaserScan() scan.header.seq = count scan.header.stamp = t1 scan.header.frame_id = front_rear scan.angle_min = -1.57 scan.angle_max = 1.57 scan.angle_increment = 3.14 / 180 scan.time_increment = (1.0 / 75.0)/181 scan.range_min = 0.0 scan.range_max = 30.0 scan.ranges = [] scan.intensities = [] for j in range(3, 184):
可视化颜色阈值调参软件 ''' import cv2 import numpy as np import math import sys import rospy from sensor_msgs.msg import Image from cv_bridge import CvBridge from sensor_msgs.msg import LaserScan pub_Img = rospy.Publisher('circle/image', Image, queue_size=1) pubScan = rospy.Publisher('/circle/scan', LaserScan, queue_size=1) circleScan = LaserScan() #rospy.init_node('detectCircle', anonymous=True) bridge = CvBridge() def image_callback(msg): img = bridge.imgmsg_to_cv2(msg, "bgr8") #img = cv2.resize(img,(160,128)) pub_Img.publish(bridge.cv2_to_imgmsg(img, '8UC3')) def scan_callback(msg): circle_distance = [] circle_angle = [] #print(len(msg.ranges)) #1440 max for i in range(len(msg.ranges)):
def __init__(self): self.sim = rospy.get_param('~sim', True) self.version = rospy.get_param('~version', 0) self.model = rospy.get_param('~model', 'frozen_model.pb') self.graph = tf.Graph() my_dir = os.path.abspath(os.path.dirname(__file__)) PATH_TO_CKPT = os.path.join( my_dir, "../model/" + self.model) with self.graph.as_default(): od_graph_def = tf.GraphDef() with tf.gfile.GFile(PATH_TO_CKPT, 'rb') as fid: serialized_graph = fid.read() od_graph_def.ParseFromString(serialized_graph) tf.import_graph_def(od_graph_def, name='') config = tf.ConfigProto() config.gpu_options.allow_growth = True self.sess = tf.Session(graph=self.graph, config=config) self.actions = [] self.linear_pid = PID_control(p_name='linear', P=1, I=0, D=0) self.angular_pid = PID_control(p_name='angular', P=1, I=0, D=0) self.topic_name = '' if not self.sim: self.linear_pid = PID_control(p_name='linear', P=1, I=0, D=0) self.angular_pid = PID_control(p_name='angular', P=1, I=0, D=0) self.topic_name = '/husky_velocity_controller/cmd_vel' if self.version == 0: self.actions = [[0.1, -0.35], [0.3, -0.35], [0.3, -0.2], [0.3, 0.0], [0.3, 0.2], [0.3, 0.35], [0.1, 0.35]] elif self.version == 1: max_ang_speed = 0.4 max_linear = 0.35 for action in range(21): ang_vel = (action-10)*max_ang_speed*0.1 lin_vel = max_linear-((action-10)/2.6)**2*0.1 self.actions.append([lin_vel, ang_vel]) else: self.linear_pid = PID_control(p_name='linear', P=0.8, I=0, D=0) self.angular_pid = PID_control(p_name='angular', P=0.8, I=0, D=0) self.topic_name = '/X1/cmd_vel' if self.version == 0: self.actions = [[0.5, -0.8], [1.5, -0.8], [1.5, -0.4], [1.5, 0.0], [1.5, 0.4], [1.5, 0.8], [0.5, 0.8]] elif self.version == 1: max_ang_speed = 0.8 for action in range(21): ang_vel = (action-10)*max_ang_speed*0.1 lin_vel = 1.5-((action-10)/2.6)**2*0.1 self.actions.append([lin_vel, ang_vel]) self.pub_twist = rospy.Publisher(self.topic_name, Twist, queue_size=1) self.laser_upper = LaserScan() self.laser_lower = LaserScan() self.sub_laser_upper = rospy.Subscriber( '/RL/scan/upper', LaserScan, self.cb_laser_upper, queue_size=1) self.sub_laser_lower = rospy.Subscriber( '/RL/scan/lower', LaserScan, self.cb_laser_lower, queue_size=1) self.input = self.graph.get_tensor_by_name('eval_net/input/s:0') self.output_q = self.graph.get_tensor_by_name( 'eval_net/l3/output_q:0') self.last_cmd = [0, 0] self.timer = rospy.Timer(rospy.Duration(0.1), self.timer_pub)
def range_to_laser_scan(self): """ Header header # timestamp in the header is the acquisition time of # the first ray in the scan. # # in frame frame_id, angles are measured around # the positive Z axis (counterclockwise, if Z is up) # with zero angle being forward along the x axis float32 angle_min # start angle of the scan [rad] float32 angle_max # end angle of the scan [rad] float32 angle_increment # angular distance between measurements [rad] float32 time_increment # time between measurements [seconds] - if your scanner # is moving, this will be used in interpolating position # of 3d points float32 scan_time # time between scans [seconds] float32 range_min # minimum range value [m] float32 range_max # maximum range value [m] float32[] ranges # range data [m] (Note: values < range_min or > range_max should be discarded) float32[] intensities # intensity data [device-specific units]. If your # device does not provide intensities, please leave # the array empty. """ # actualrange = 0 <--> 125 degrees msg = LaserScan() msg.header.frame_id = "radar" msg.header.stamp = rospy.Time.now() msg.range_max = 4.00 msg.range_min = 0.02 msg.angle_min = math.radians(0) msg.angle_max = math.radians(360) # msg.angle_min = math.radians(-90) # msg.angle_max = math.radians((270)) msg.angle_increment = math.radians(0.7) msg.ranges = [4.0] * int( (math.degrees(msg.angle_max) - math.degrees(msg.angle_min)) / 0.7) # radius / increment = number of samples time_started = rospy.Time.now() for a in range(0 if self.radar_toggle else 180, 180 if self.radar_toggle else 0, 1 if self.radar_toggle else -1): self.set_angle(a) # time.sleep(0.1) for name, distance in self.sensors.distance_all(): if name == "right": msg.ranges[a + self.spare_degrees] = min( [msg.ranges[a + self.spare_degrees], distance]) else: msg.ranges[a + 256 + self.spare_degrees] = min( [msg.ranges[a + 256 + self.spare_degrees], distance]) elapsed = (rospy.Time.now() - time_started).to_sec() msg.time_increment = elapsed # if not self.radar_toggle: # if its the opposite scan, reverse the list # # msg.ranges = msg.ranges[:len(msg.ranges)//2:-1]+msg.ranges[len(msg.ranges)//2::-1] # msg.ranges = msg.ranges[::-1] self.pub.publish(msg) self.radar_toggle = not self.radar_toggle
def __init__(self): """Initialization.""" # initialize the node rospy.init_node( 'hector_controller', anonymous=True # name of the node ) # param from launch file self.name = 'hector10' # tell ros to call stop when the program is terminated rospy.on_shutdown(self.kill) # create velocity publisher self.velocity_publisher = rospy.Publisher( '/cmd_vel', # name of the topic Twist, # message type queue_size=10 # queue size ) # create height subscriber self.sonar_height_subscriber = rospy.Subscriber( '/sonar_height', # name of the topic Range, # message type self.update_sonar_height # function that hanldes incoming messages ) # create height subscriber self.laser_scanner_subscriber = rospy.Subscriber( '/scan', # name of the topic LaserScan, # message type self. update_laser_scanner # function that hanldes incoming messages ) # create goal client self.goal_client = actionlib.SimpleActionClient( '/action/pose', hector_uav_msgs.msg.PoseAction, ) # initialize sonar height to 0 self.sonar_height = Range().range # initialize laser scanner # LaserScan.ranges consists of 1081 values (array/list) # The implemented laser scanner scans from -3/4*pi to +3/4*pi (min_angle, max_angle) self.laser_scanner = LaserScan() # initialize linear and angular velocities to 0 self.velocity = Twist() # set node update frequency in Hz self.rate = rospy.Rate(10) self.path = [[1, -1], [0, -1], [-0.5, -1], [-1, -1], [-1.5, -1], [-2, -1], [-2.5, -1], [-3, -1], [-3, -0.5], [-3, 0], [-3, 0.5], [-3, 1], [-3, 1.5], [-3, 2], [-3, 2.5], [-2.5, 3], [-2, 3], [-1.5, 3], [-1, 3], [-0.5, 3], [0, 3], [0.5, 3], [1, 3], [1, 2.5], [1, 2], [1, 1.5], [1, 1], [1, 0.5], [1, 0], [1, -0.5], [1, -1]]
def on_lidar_100_data(self, topic=LaserScan()): angle_min = topic.angle_min angle_max = topic.angle_max assert angle_min == -angle_max self.back_angle_and_distance = self.process_lidar_data( topic.ranges, angle_max)
def __init__(self): self.points = Marker() self.line_strip = Marker() self.goal_circle = Marker() self.odom_goal_pos = Point() self.odom = Odometry() self.map_path = Path() self.mux_msg = muxMsg() self.mg = muxMsg() self.scan_date = LaserScan() self.L = rospy.get_param('~L', default=0.335) self.Vcmd = rospy.get_param('~Vcmd', default=1.5) self.lfw = rospy.get_param('~lfw', default=0.13) self.controller_freq = rospy.get_param('~controller_freq', default=20) self.base_angle = rospy.get_param('~baseAngle', 90.0) rospy.Subscriber('/scan', LaserScan, self.scan_cb) # rospy.Subscriber('/odom',Odometry,self.odom_cb) rospy.Subscriber('/odometry/filtered', Odometry, self.odom_cb) rospy.Subscriber('/move_base/TebLocalPlannerROS/local_plan_odom', Path, self.path_cb) rospy.Subscriber('/move_base_simple/goal_odom', PoseStamped, self.goal_cb) rospy.Subscriber('/control/close_loop_msg', muxMsg, self.mux_cb) rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.amcl_pose_cb) self.marker_pub = rospy.Publisher('/control/car_path', Marker, queue_size=10) self.cmd_pub = rospy.Publisher('/control/cmd_vel', Twist, queue_size=2) self.goal_reached_pub = rospy.Publisher('/move_base_simple/isgetGoal', muxMsg, queue_size=10) self.amcl_pose = PoseWithCovarianceStamped() self.goalRadius = 0.2 # 车距离目标点的距离半径 self.Lfw = 0.8 # self.getL1Distance(self.Vcmd) #前视距离和速度成正比 self.foundForwardPt = self.goal_received = self.goal_reached = False self.cmd_vel = Twist() self.cmd_vel.linear.x = 1500 self.cmd_vel.angular.z = self.base_angle # 前视角 self.eta_pre = 0 # p i d Lfw self.move_steer_pid = { 1: (4, 0, 1, 1), 1.5: (4, 0, 1, 1), 2: (4, 0, 1, 1.1), 2.5: (3, 0, 1, 1.2), 3: (3, 0, 1, 1.25), 3.5: (3, 0, 1, 1.3), 4: (3, 0, 1, 1.35), 4.5: (3, 0, 1, 1.4), 5: (3, 0, 1, 1.45) } # 舵机控制及速度控制pid参数 self.steer_kp = self.steer_ki = self.steer_kd = self.steer_p = self.steer_i = self.steer_d = self.steer_error = self.steer_pre_error = 0 self.steering_angle = 0 # 应该输出的舵机角度 # p i d pwm self.move_speed_pid = { 1: (30, 0, 10, 1500), 1.5: (35, 0, 15, 1500), 2: (38, 0, 20, 1500), 2.5: (35, 0, 25, 1500), 3: (40, 0, 30, 1500), 3.5: (50, 0, 35, 1500), 4: (55, 0, 38, 1500), 4.5: (60, 0, 41, 1500), 5: (65, 0, 45, 1500) } # 前进pid self.speed_kp = self.speed_ki = self.speed_kd = self.speed_p = self.speed_i = self.speed_d = self.speed_error = self.speed_pre_error = 0 self.speed_v = self.Vcmd # 应该达到的速度 rospy.loginfo("[param] base_angle: %f", self.base_angle) rospy.loginfo("[param] Vcmd: %f", self.Vcmd) rospy.loginfo("[param] Lfw: %f", self.Lfw) rospy.loginfo("[param] lfw: %f", self.lfw) self.init_marker() rate = rospy.Rate(self.controller_freq) while not rospy.is_shutdown(): self.goalReaching() self.controlLoop() rate.sleep()
def spin(self): encoders = [0, 0] self.x = 0 # position in xy plane self.y = 0 self.th = 0 then = rospy.Time.now() # things that don't ever change scan_link = rospy.get_param('~frame_id', 'base_laser_link') scan = LaserScan(header=rospy.Header(frame_id=scan_link)) scan.angle_min =0.0 scan.angle_max =359.0*pi/180.0 scan.angle_increment =pi/180.0 scan.range_min = 0.020 scan.range_max = 5.0 odom = Odometry(header=rospy.Header(frame_id="odom"), child_frame_id='base_footprint') dist = Vector3Stamped(header=rospy.Header(frame_id="odom")) button = Button() sensor = Sensor() self.robot.setBacklight(1) self.robot.setLED("Green") # main loop of driver r = rospy.Rate(20) cmd_rate= self.CMD_RATE self.prevRanges = [] while not rospy.is_shutdown(): # notify if low batt #if self.robot.getCharger() < 10: # print "battery low " + str(self.robot.getCharger()) + "%" # get motor encoder values left, right = self.robot.getMotors() cmd_rate = cmd_rate-1 if cmd_rate ==0: # send updated movement commands #if self.cmd_vel != self.old_vel or self.cmd_vel == [0,0]: # max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1]))) #self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], (abs(self.cmd_vel[0])+abs(self.cmd_vel[1]))/2) self.robot.setMotors(self.cmd_vel[0], self.cmd_vel[1], max(abs(self.cmd_vel[0]),abs(self.cmd_vel[1]))) cmd_rate = self.CMD_RATE self.old_vel = self.cmd_vel # prepare laser scan scan.header.stamp = rospy.Time.now() self.robot.requestScan() scan.ranges = self.robot.getScanRanges() # now update position information dt = (scan.header.stamp - then).to_sec() then = scan.header.stamp d_left = (left - encoders[0])/1000.0 d_right = (right - encoders[1])/1000.0 encoders = [left, right] #print d_left, d_right, encoders dx = (d_left+d_right)/2 dth = (d_right-d_left)/(self.robot.base_width/1000.0) x = cos(dth)*dx y = -sin(dth)*dx self.x += cos(self.th)*x - sin(self.th)*y self.y += sin(self.th)*x + cos(self.th)*y self.th += dth #print self.x,self.y,self.th # prepare tf from base_link to odom quaternion = Quaternion() quaternion.z = sin(self.th/2.0) quaternion.w = cos(self.th/2.0) # prepare odometry # synchronize /odom and /scan in time odom.header.stamp = rospy.Time.now() + rospy.Duration(0.1) odom.pose.pose.position.x = self.x odom.pose.pose.position.y = self.y odom.pose.pose.position.z = 0 odom.pose.pose.orientation = quaternion odom.twist.twist.linear.x = dx/dt odom.twist.twist.angular.z = dth/dt dist.header.stamp = odom.header.stamp dist.vector.x = encoders[0]/1000 dist.vector.y = encoders[1]/1000 # sensors lsb, rsb, lfb, rfb = self.robot.getDigitalSensors() # buttons btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down = self.robot.getButtons() # publish everything self.odomBroadcaster.sendTransform((self.x, self.y, 0), (quaternion.x, quaternion.y, quaternion.z, quaternion.w), then, "base_footprint", "odom") if self.prevRanges != scan.ranges: self.scanPub.publish(scan) self.prevRanges = scan.ranges self.odomPub.publish(odom) self.distPub.publish(dist) button_enum = ("Soft_Button", "Up_Button", "Start_Button", "Back_Button", "Down_Button") sensor_enum = ("Left_Side_Bumper", "Right_Side_Bumper", "Left_Bumper", "Right_Bumper") for idx, b in enumerate((btn_soft, btn_scr_up, btn_start, btn_back, btn_scr_down)): if b == 1: button.value = b button.name = button_enum[idx] self.buttonPub.publish(button) for idx, b in enumerate((lsb, rsb, lfb, rfb)): if b == 1: sensor.value = b sensor.name = sensor_enum[idx] self.sensorPub.publish(sensor) # wait, then do it again r.sleep() # shut down self.robot.setBacklight(0) self.robot.setLED("Off") self.robot.setLDS("off") self.robot.setTestMode("off")
def scanmatcher_worker(self): # vars has_initial_scan = False # initialize ICP icp_options = pymrpt.slam.CICP.TConfigParams() icp = pymrpt.slam.CICP(icp_options) # create last and current laser maps last_map = pymrpt.maps.CSimplePointsMap() # setup last update time last_update_time = rospy.Time.now() # loop while not rospy.is_shutdown(): self._scan_msg = LaserScan() # get scan from queue try: item = self._scan_queue.get(timeout=5) except Empty: rospy.logwarn( 'Got no scan from queue since 5 seconds. Scanmatcher will shutdown now!' ) continue self._scan_msg = item[1] self._scan_msg.ranges = list(self._scan_msg.ranges) rospy.loginfo('received scan...') # update current stamp for publishers self._current_stamp = self._scan_msg.header.stamp # get laser scanner pose laser_pose, ok = self.get_base_link_base_laser_tf() self._sensor_pose = pymrpt.poses.CPose3D() self._sensor_pose.from_ROS_Pose_msg(laser_pose.pose) # convert data observation = pymrpt.obs.CObservation2DRangeScan() observation.from_ROS_LaserScan_msg(self._scan_msg, self._sensor_pose) # set current map from scan current_map = pymrpt.maps.CSimplePointsMap() current_map.loadFromRangeScan(observation) # match maps if has_initial_scan: # no initial guess (pure incremental) initial_guess = pymrpt.poses.CPosePDFGaussian() # run ICP algorithm pose_change, running_time, info = icp.AlignPDF( last_map, current_map, initial_guess) rospy.loginfo('icp goodness: {}'.format(info.goodness)) rospy.loginfo('icp run_time: {}'.format(running_time)) rospy.loginfo('pose change: {}'.format(pose_change.mean)) # check goodness # if info.goodness > .8: rospy.loginfo('...updating odometry.') # get time delta d_t = (last_update_time - self._current_stamp).to_sec() # update current pose and velocities self._current_pose += pose_change.mean dist = pose_change.mean.norm() self._v = dist / d_t self._w = pose_change.mean.phi / d_t self.publish_odom() # update last update time last_update_time = self._current_stamp # update last map last_map = current_map # else: # rospy.logwarn('...lost odometry...') else: rospy.loginfo('...is inital one!') # load initial scan to last map last_map = current_map # mark initial as received has_initial_scan = True # rospy.loginfo('...task done!') # signalize work done self._scan_queue.task_done()
def __init__(self): rospy.Subscriber('cmd_vel', Twist, self.cmdvel_callback) laser_pub = rospy.Publisher('scan', LaserScan, queue_size=1) enc_pub = rospy.Publisher('encoder', Encoder, queue_size=1) # pose_pub = rospy.Publisher('pose', PoseStamped, queue_size=1) sonar_pub = rospy.Publisher('range_dist', Sonar, queue_size=1) self.enc_msg = Encoder() scan_msg = LaserScan() sonar_msg = Sonar() self.enc_msg.vx = 0 self.enc_msg.w = 0 scan_msg.angle_increment = 0.00581718236208 scan_msg.angle_max = 2.35572338104 scan_msg.angle_min = -2.35619449615 scan_msg.range_max = 10.0 scan_msg.range_min = 0.05 scan_msg.scan_time = 0.06666666 scan_msg.header.frame_id = 'laser' with open('/home/whj/catkin_ws/src/api/api/data/scan2.txt') as f: scan_msg.ranges = map(float, f.readline().split(',')) scan_msg.intensities = map(float, f.readline().split(',')) # pose_msg.header.frame_id = 'base_link' # pose_msg.pose.position.x = 3.1 # pose_msg.pose.position.y = -0.1 # pose_msg.pose.position.z = 0 # pose_msg.pose.orientation.w = 1.0 # pose_msg.pose.orientation.x = 0 # pose_msg.pose.orientation.y = 0 # pose_msg.pose.orientation.z = 0 sonar_msg.ranges = [20, 30, 40, 50, 60, 70] rate = rospy.Rate(15) while not rospy.is_shutdown(): # pose_msg.pose.position.x += 0.01 # pose_msg.pose.position.y += 0.01 # pose_msg.pose.orientation.z += 0.01 # pose_msg.pose.orientation.w = 1 - pose_msg.pose.orientation.z**2 # if pose_msg.pose.position.x > 6.7 or pose_msg.pose.position.y > 9.0: # pose_msg.pose.position.x = 3.0 # pose_msg.pose.position.y = -3.5 # if pose_msg.pose.orientation.w < 0.6: # pose_msg.pose.orientation.z = 0.01 #enc_msg.vx = 0 #enc_msg.w = 200 scan_msg.header.stamp = rospy.Time.now() sonar_msg.ranges[0] = sonar_msg.ranges[ 0] + 0.1 if sonar_msg.ranges[0] < 80 else 20 sonar_msg.ranges[1] = sonar_msg.ranges[ 1] + 0.1 if sonar_msg.ranges[1] < 80 else 20 sonar_msg.ranges[2] = sonar_msg.ranges[ 2] + 0.1 if sonar_msg.ranges[2] < 80 else 20 sonar_msg.ranges[3] = sonar_msg.ranges[ 3] + 0.1 if sonar_msg.ranges[3] < 80 else 20 sonar_msg.ranges[4] = sonar_msg.ranges[ 4] + 0.1 if sonar_msg.ranges[4] < 80 else 20 sonar_msg.ranges[5] = sonar_msg.ranges[ 5] + 0.1 if sonar_msg.ranges[5] < 80 else 20 enc_pub.publish(self.enc_msg) laser_pub.publish(scan_msg) sonar_pub.publish(sonar_msg) rate.sleep()
#!/usr/bin/env python import rospy from subsomption.msg import Channel from sensor_msgs.msg import LaserScan from std_msgs.msg import Bool from math import * from custom_strategy_gating import freq, ratio # @CHANGED import random bumper_l = 0 bumper_r = 0 lasers = LaserScan() def callback_right_bumper(data): global bumper_r bumper_r = data.data #rospy.loginfo(rospy.get_caller_id()+"Right bumper %d",data.data) def callback_left_bumper(data): global bumper_l bumper_l = data.data #rospy.loginfo(rospy.get_caller_id()+"Left bumper %d",data.data) def callback_lasers(data): global lasers lasers = data
def __init__(self, in_scan_topic='/scan', expected_scan_topic='/expected_scan', out_scan_topic='/filtered_scan', detected_object_topic='/lidar_objects', amcl_pose_topic='/amcl_pose', odom_topic='/odom', threshold=0.05, window_size=3, window_step=2, covariance_threshold=0.007, frame_id='base_scan', size_threshold=0.1): """ Initializes object params ------ in_scan_topic : str Real scan topic expected_scan_topic : str Expected scan topic out_scan_topic : str Filtered scan topic to go to localization nodes detected_object_topic : str Topic to publish detected objects to pose_topic : str Topic to subscribe to pose threshold: float Error threshold from cross correlation to count as a new object window_size: int Sliding window size. Window is slid across both signals, getting an cross correlation error window_step: int Window step size. covariance_threshold: float Any pose with a covariance higher than this threshold will cause the real scan to be passed through and no objects to be detected. frame_id: str Name of the tf frame of the scanner (for publishing distance) size_threshold: float Objects smaller than this size will not be published """ rospy.Subscriber(in_scan_topic, LaserScan, self.ls_callback) rospy.Subscriber(expected_scan_topic, LaserScan, self.els_callback) rospy.Subscriber(amcl_pose_topic, PoseStamped, self.pose_callback) rospy.Subscriber(odom_topic, Odometry, self.odom_callback) self.out_scan_pub = rospy.Publisher(out_scan_topic, LaserScan, queue_size=5) self.detected_object_pub = rospy.Publisher(detected_object_topic, NovelObjectArray, queue_size=5) self.last_scan = np.array([]) self.last_expected = np.array([]) self.last_pose = Pose() self.last_scan_msg = LaserScan() self.window_size = window_size self.window_step = window_step self.threshold = threshold self.size_threshold = size_threshold self.range_max = 1 self.covariance_threshold = covariance_threshold self.frame_id = frame_id self.pose_ready = False self.odom_topic = odom_topic self.moving = False
from defs import * from std_srvs.srv import Empty ''' pose.pose.position.x = data.pose.position.x pose.pose.position.y = data.pose.position.y pose.pose.position.z = data.pose.position.z #pose.pose.orientation.x = data.pose.orientation.x #pose.pose.orientation.y = data.pose.orientation.y #pose.pose.orientation.z = data.pose.orientation.z #pose.pose.orientation.w = data.pose.orientation.w ''' tag_center_pose = PoseStamped() amcl_pose = PoseWithCovarianceStamped() scansione = LaserScan() startstop = 0.0 th_laser = 100 def vicon_cb_0(data): global tag_center_pose tag_center_pose = data def vicon_cb_1(data): global amcl_pose amcl_pose = data def lidar_cb(data):
from mavros_msgs.srv import CommandBool from mavros_msgs.srv import CommandTOL from mavros_msgs.srv import SetMode #parameters d_max=30 k_att_rep=5 #ratio of attractive force to repulsive force speed=5 #Max speed of drone threshold_vel=0.5 #minimum speed at which to declare minima , ranges from 0 to speed n=4 #paramater decreasing repulsive step_size = 0.5 #distance traveled by drone in each step threshold_force = 5 alpha= 1#weightage of theta_center vs theta_goal temp=True pose = PoseStamped() lidar = LaserScan() vel = Twist() setpoint = PoseStamped() initial_orientation = 0 stuck = False def callback_lidar(msg): global lidar lidar = msg def distance(point_1,point_2): return np.sqrt( (point_1[0]-point_2[0])**2 + (point_1[1]-point_2[1])**2 ) def callback_pose(msg): global pose