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
