[-0.017 ,0.046 , 0.017 ,0.999 ],
[-0.017 ,0.045 , 0.016 ,0.999 ],
[-0.016 ,0.045 , 0.017 ,0.999 ],
[-0.016 ,0.043 , 0.017 ,0.999 ],
[-0.016 ,0.042 , 0.017 ,0.999 ],
[-0.016 ,0.042 , 0.017 ,0.999 ],
[-0.016 ,0.041 , 0.018 ,0.999 ],
[-0.015 ,0.042 , 0.019 ,0.999 ],
[-0.015 ,0.043 , 0.019 ,0.999 ],
[-0.014 ,0.043 , 0.019 ,0.999 ],
[-0.014 ,0.044 , 0.019 ,0.999 ],
[-0.013 ,0.043 , 0.018 ,0.999 ],
[-0.012 ,0.043 , 0.018 ,0.999 ],
[-0.011 ,0.043 , 0.017 ,0.999 ],
[-0.010 ,0.042 , 0.016 ,0.999 ] ]
last_data = Float32MultiArray()
last_data.data.extend([0.0, 0.0, 0.0, 1.0])
started = False
pub = rospy.Publisher('kumara/head/pose/mbed', Float32MultiArray, queue_size=1000)
def listener():
global started, pub, last_data
rospy.init_node('fuvkyou', anonymous=False)
for i in pop:
last_data.data = i
pub.publish(last_data)
print(last_data)
def move_arm(self, position):
rospy.loginfo("[Decon] Starting to position arm...")
request = Float32MultiArray()
request.data = position
self.arm_command_topic.publish(request)
def __init__(self):
parser = argparse.ArgumentParser()
parser.add_argument(
'--cfg',
type=str,
default='/module/src/rectifier/rectifier/cfg/yolov3.cfg',
help='*.cfg path')
parser.add_argument('--names',
type=str,
default='data/coco.names',
help='*.names path')
parser.add_argument(
'--weights',
type=str,
default='/module/src/rectifier/rectifier/weights/yolov3.pt',
help='weights path')
parser.add_argument('--source',
type=str,
default='data/samples',
help='source')
parser.add_argument('--output',
type=str,
default='output',
help='output folder') # output folder
parser.add_argument('--img-size',
type=int,
default=416,
help='inference size (pixels)')
parser.add_argument('--conf-thres',
type=float,
default=0.3,
help='object confidence threshold')
parser.add_argument('--iou-thres',
type=float,
default=0.6,
help='IOU threshold for NMS')
parser.add_argument('--fourcc',
type=str,
default='mp4v',
help='output video codec (verify ffmpeg support)')
parser.add_argument('--half',
action='store_true',
help='half precision FP16 inference')
parser.add_argument('--device',
default='0',
help='device id (i.e. 0 or 0,1) or cpu')
parser.add_argument('--view-img',
action='store_true',
help='display results')
parser.add_argument('--save-txt',
action='store_true',
help='save results to *.txt')
parser.add_argument('--classes',
nargs='+',
type=int,
help='filter by class')
parser.add_argument('--agnostic-nms',
action='store_true',
help='class-agnostic NMS')
self.opt = parser.parse_args()
# self.opt = args_
print(self.opt)
# (320, 192) or (416, 256) or (608, 352) for (height, width)
img_size = (320, 192) if ONNX_EXPORT else self.opt.img_size
out, source, weights, self.half, view_img, save_txt = self.opt.output, self.opt.source, self.opt.weights, self.opt.half, self.opt.view_img, self.opt.save_txt
self.device = select_device(
device='cpu' if ONNX_EXPORT else self.opt.device)
self.model = Darknet(self.opt.cfg, img_size)
self.model.load_state_dict(
torch.load(weights, map_location=self.device)['model'])
# Second-stage classifier
self.classify = False
if self.classify:
modelc = load_classifier(name='resnet101', n=2) # initialize
modelc.load_state_dict(
torch.load('weights/resnet101.pt',
map_location=device)['model']) # load weights
modelc.to(self.device).eval()
# Eval mode
self.model.to(self.device).eval()
self.tl_bbox = Float32MultiArray()
self.arry_size = 6
super().__init__('talker')
self._pub = self.create_publisher(Float32MultiArray, '/tl_bbox_info',
10)
self._cv_bridge = CvBridge()
super().__init__('rectifier')
self._sub = self.create_subscription(
Image, '/snowball/perception/traffic_light/cropped_roi',
self.img_callback, 10)
print(
"ready to process rectifier----------------------------------------------------------"
)
def callback(datas):
global inn
global dest_x_global
global dest_y_global
global dest_y_stair
global total_g
global fake_void
if (inn >= 6):
inn = 0
dist = np.empty((480, 640), dtype=np.uint16)
imagem = np.empty((480, 640), dtype=np.uint8)
k = 0
valor_medio = 0
dist_media = 0
dist_esc = 0
total = 0
begin = 0
void = 0
is_void = False
stair = False
initial_h = 0
valor_medio_e = 0
desv = 0
msg = rospy.Publisher('/object_detection',
Float32MultiArray,
queue_size=1)
pub = Float32MultiArray()
# Matrix of the kinect image
for i in range(480):
for j in range(640):
dist[i, j] = ord(
datas.data[2 * k]) + (ord(datas.data[2 * k + 1]) << 8)
imagem[i, j] = dist[i, j] // 13.6718751
k += 1
# Method to find one end of the obstacle
for k in range(480):
if imagem[k][10] != 255:
k -= 30
break
for l in range(380):
if imagem[k - 10][l] != 255 and begin == 0:
begin = l
if imagem[k - 10][l] == 255 and begin != 0 and void == 0:
void = l + 10
is_void = True
break
# Identification of the restrict area
if begin != 0:
if is_void:
for p in range(k, 100, -1):
if imagem[p][void] != 255:
is_void = False
break
# Calculate the the average distance of the region of pixels
for m in range(k - 10, k):
for n in range(begin, begin + 10):
total += 1
valor_medio += imagem[m][n + 30]
dist_media += dist[m][n + 30]
dist_esc += dist[m][n]
imagem[m][n] = 0
# Identification of the stair
if valor_medio / total > 240:
for o in range(k, 480):
if imagem[o][begin] != 255:
stair = True
break
if stair:
for q in range(begin + 50, 630):
if imagem[k][q] != 255:
initial_h = q
break
for m in range(k - 10, k):
for r in range(initial_h, initial_h + 10):
valor_medio_e += dist[m][r]
imagem[m][r] = 0
desv = (((320 - begin) * 0.00173667 * (dist_esc / total)) +
((320 - initial_h) * 0.00173667 *
(valor_medio_e / total))) / 2
# Calculates the destination spot
dest_x = -dist_esc / (total * 1000) + gps_x_global + 1
dest_y = desv / 1000 + gps_y_global
else:
if is_void:
# Calculates the destination spot
if gps_y_global < 0:
dest_x = dist_media / (total *
1000) + gps_x_global - 0.4248
dest_y = (void - 320) * 0.00173667 * (
dist_esc / (total * 1000)) + 0.2 + gps_y_global
print dest_y
if dest_y > -1.8:
fake_void = True
if gps_y_global > 0:
dest_x = -dist_media / (total *
1000) + gps_x_global + 0.4248
dest_y = (void - 320) * 0.00173667 * (
dist_esc /
(total * 1000)) * (-1) - 0.2 + gps_y_global
print dest_y
if dest_y < 1.7:
fake_void = True
if dist_media / total < 2300 and dist_media / total > 1850 and not fake_void:
dest_x_global += dest_x
dest_y_global += dest_y
total_g += 1
elif total_g != 0 and dist_media / total < 1850 and not fake_void:
dest_x_global /= total_g
dest_y_global /= total_g
# void identificator, X destnation coordinate, Y destnation coordinate
arg = (1, dest_x_global, dest_y_global)
pub.data = arg
msg.publish(pub)
total_g = 0
dest_x_global = 0
dest_y_global = 0
if not is_void or fake_void:
# Calculates the destination spot
if gps_y_global < 0:
dest_x = dist_media / (total *
1000) + gps_x_global - 0.3248
dest_y = (begin - 320) * 0.00173667 * (
dist_esc / (total * 1000)) - 1.0 + gps_y_global
if gps_y_global > 0:
dest_x = -dist_media / (total *
1000) + gps_x_global + 0.3248
dest_y = ((begin - 320) * 0.00173667 *
(dist_esc /
(total * 1000)) - 1.2) * (-1) + gps_y_global
if dist_media / total < 2500 and dist_media / total > 2300:
dest_x_global += dest_x
dest_y_global += dest_y
total_g += 1
elif total_g != 0 and dist_media / total < 2300:
dest_x_global /= total_g
dest_y_global /= total_g
# obstacle identificator, X destnation coordinate, Y destnation coordinate
arg = (2, dest_x_global, dest_y_global)
pub.data = arg
msg.publish(pub)
total_g = 0
dest_x_global = 0
dest_y_global = 0
self.state = "EXIT"
self.rate.sleep()
#Home coordinates: [2.7955,-0.41917,0,-24.27]
if __name__ == '__main__':
try:
home_coord = [3.2278,-0.94701,0,62]
#dcontroller = DeconController([[0.4,-1.06,0,0], [0.4,-0.532,0], [0.5,-1.5,0,-90]])
#dcontroller = DeconController([[3.49093,-1.12527,0,-26.5],[2.883,-0.72133,0,65.91]])
dcontroller = DeconController([[3.2278,-0.94701,0,62]])
#[2.883,-0.72133,0,65.91]])
dcontroller.start()
dcontroller.move_arm([0.0, 0.0])
ret = dcontroller.wait_for(ARM_FLAG)
if ret is True:
ret = False
request = Float32MultiArray()
request.data = home_coord
dcontroller.move2next_location_command_topic.publish(request)
ret = dcontroller.wait_for(LOCATION_FLAG)
if ret is not True:
rospy.loginfo("[Decon] Error Occured going back to home position...")
rospy.signal_shutdown("[Decon] Good Bye...")
except rospy.ROSInterruptException:
log_string = "[Decon] Decon Controller exiting at %s!" % rospy.get_time()
rospy.loginfo(log_string)
#!/usr/bin/python
import rospy
import numpy as np
from geometry_msgs.msg import Pose
from std_msgs.msg import Float32MultiArray
from std_msgs.msg import Float64MultiArray
# Node
rospy.init_node('matlab_path', anonymous=True)
path_publisher = rospy.Publisher('/matlab_topic',
Float32MultiArray,
queue_size=10)
path_msg = Float32MultiArray()
#path_msg.
path_msg.data = [0.0, 3.0]
"""
path_msg.data = [[2.75414116625081, 2.04761128963112],
[2.89518236041524, 3.61823207641913],
[5.03080150303697, 6.30976383559748],
[4.93753765416243, 7.30776336301881],
[5.86937727776949, 8.33166766049375],
[11.8932359182601, 10.0929765780663],
[13.2824577304770, 12.3308383615348],
[16.3616564164733, 14.8233350983323],
[16.2075912367160, 17.2198548623449],
[17, 17]]
"""
def camera_fix_publisher():
# ROS节点初始化
rospy.init_node('camera_fix_publisher', anonymous=True)
cap = cv2.VideoCapture(1)
# 创建一个Publisher，发布名为/camera_fix_info的topic，消息类型为std_msgs::Float32MultiArray，队列长度1
camera_fix_info_pub = rospy.Publisher('/camera_fix_info',
Float32MultiArray,
queue_size=1)
rospy.Subscriber("/robot_motion_info", Bool, robotCallback)
#设置循环的频率
rate = rospy.Rate(5)
speed = 0
color = 0
dx = 0.406 # image space to robot space 60.5/149
xc = 0
last_xc = 0
yc = 0
num = 0
while not rospy.is_shutdown() & cap.isOpened():
color, xc, yc = ca.Detect(cap)
# print(xc, yc, speed)
if (color > 0) & (xc > 180) & (num < 5):
if last_xc > 180 & num <= 3:
if num == 0:
speed = ((xc - last_xc) * dx / 0.2)
else:
speed = (speed + ((xc - last_xc) * dx / 0.2)) / 2
num = num + 1
print(num, last_xc, xc, yc, speed)
if last_xc > xc: # 如果物体已经检测过，就清零
speed = 0
color = 0
xc = 0
last_xc = 0
yc = 0
num = 0
last_xc = xc
if num == 4:
num += 1
global Motion
if (speed > 0.01) & (xc > 0) & (not Motion):
array = [xc, yc, speed, color]
camera_fix_info = Float32MultiArray(data=array)
rospy.loginfo(camera_fix_info.data)
# 发布消息
camera_fix_info_pub.publish(camera_fix_info)
if (num == 5) & (xc == 0):
speed = 0
color = 0
xc = 0
last_xc = 0
yc = 0
num = 0
# 按照循环频率延时
rate.sleep()
cap.release()
cv2.destroyAllWindows()
def update_state(msg):
''' for simulation '''
global pub_gps, gps_message, pub_imu, x, y, v, omega, theta, pub_odom
#uR = -uR/ 100
#uL = -uL/ 100
update_lidar()
setup_walls()
uR = -msg.strboard / 100.0
uL = -msg.port / 100.0
rudder = msg.servo
current_v = rospy.get_param('v_current', 0.5)
current_ang = rospy.get_param('current_ang', math.pi / 2)
current = Float32MultiArray()
current.data = [current_v, current_ang]
# Rudder Angle
rudder_rate = (1894.0 - 1195.0) / 90.0
rudder_ang = (rudder - 1600.0) / rudder_rate
rudder_ang = -rudder_ang / 180.0 * math.pi
b_l = 4.0 # sim linear drag
b_r = 2.5 # sim rotational drag
I_zz = 1.0 # sim moment of inertia
m = 5.0 # sim mass
robot_radius = 0.5
x_cg = 0.9
w = 20.0
dt = 0.2
# update state
a = (uR + uL) / m - b_l / m * v
# print(a)
ang_acc = -b_r / I_zz * omega + (uR +
uL) * math.sin(rudder_ang) * x_cg / I_zz
v = v + a * dt
omega = omega + ang_acc * dt
robot_vx = v * math.cos(theta) - current_v * math.cos(current_ang)
robot_vy = v * math.sin(theta) - current_v * math.sin(current_ang)
v_robot = np.array([robot_vx, robot_vy])
ang_course = math.atan2(v_robot[1], v_robot[0])
v_course = math.sqrt(np.dot(v_robot, v_robot))
omega = min(max(omega, -1.0), 1.0)
# update position
# x = x + v*math.cos(angleDiff(theta)) * dt + current_v * math.cos(angleDiff(current_ang)) * dt
# y = y + v*math.sin(angleDiff(theta)) * dt + current_v * math.sin(angleDiff(current_ang))* dt
x = x + robot_vx * dt
y = y + robot_vy * dt
theta = angleDiff(theta + omega * dt)
gps_message.x = x
gps_message.y = y
gps_message.x_vel = robot_vx
gps_message.y_vel = robot_vy
gps_message.ang_course = ang_course
orig_lat = rospy.get_param('originLat', 0.0)
orig_lng = rospy.get_param('originLon', 0.0)
latlng = XYtoGPS(orig_lat, orig_lng, x, y)
gps_message.latitude = latlng[0]
gps_message.longitude = latlng[1]
# quat = qfe(0,0,theta)
# pose.pose.position.x = x
# pose.pose.position.y = y
# pose.pose.position.z = 0.0
# pose.pose.orientation.x = quat[0]
# pose.pose.orientation.y = quat[1]
# pose.pose.orientation.z = quat[2]
# pose.pose.orientation.w = quat[3]
# pose.header.frame_id = "/map"
pub_gps.publish(gps_message)
pub_imu.publish(theta)
pub_adcp.publish(current)
# pub_pose.publish(pose)
# print("x: ", x)
# print("y: ", y)
# print("theta: ", theta)
# print("xvel: ", robot_vx)
# print("yvel: ", robot_vy)
# print("ang_course: ", ang_course)
# print("current_v", current_v)
# print("current_ang", current_ang)
# odom message
odom_msg = Odometry()
odom_msg.pose.pose.position.x = x
odom_msg.pose.pose.position.y = y
quat = tf.transformations.quaternion_from_euler(0, 0, theta)
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
odom_msg.header.frame_id = 'map'
pub_odom.publish(odom_msg)
# print(state.v)
# print(state.theta)
print("x, y:", x, y)
print('current: ', current.data[1])
#! /usr/bin/env python
import rospy
import math
import time
from std_msgs.msg import Float32MultiArray, MultiArrayLayout
# Create node
rospy.init_node('test_tigrillo', anonymous=True)
pub = rospy.Publisher('tigrillo/legs_cmd', Float32MultiArray, queue_size=1)
# In a loop, send a actuation signal
t = 0
while True:
val = [-math.pi/8 * math.sin(0.01*t), math.pi/8 * math.sin(0.01*t), \
math.pi/8 * math.sin(0.01*t), -math.pi/8 * math.sin(0.01*t)]
rospy.loginfo("Publishing in tigrillo/legs_cmd topic: " + str(val))
pub.publish(Float32MultiArray(layout=MultiArrayLayout([], 1), data=val))
time.sleep(0.001)
t += 1
if t > 20000:
exit()
def servo_to_tag(self,
pose_goal,
goal_error=[0.03, 0.03, 0.1],
initial_ar_pose=None):
lost_tag_thresh = 0.6 #0.4
# TODO REMOVE
err_pub = rospy.Publisher("servo_err", Float32MultiArray)
if False:
self.test_move()
return "aborted"
#######################
goal_ar_pose = homo_mat_from_2d(*pose_goal)
rate = 20.
kf_x = ServoKalmanFilter(delta_t=1. / rate)
kf_y = ServoKalmanFilter(delta_t=1. / rate)
kf_r = ServoKalmanFilter(delta_t=1. / rate)
if initial_ar_pose is not None:
ar_err = homo_mat_to_2d(
homo_mat_from_2d(*initial_ar_pose) * goal_ar_pose**-1)
kf_x.update(ar_err[0])
kf_y.update(ar_err[1])
kf_r.update(ar_err[2])
print "initial_ar_pose", initial_ar_pose
pid_x = PIDController(k_p=0.5, rate=rate, saturation=0.05)
pid_y = PIDController(k_p=0.5, rate=rate, saturation=0.05)
pid_r = PIDController(k_p=0.5, rate=rate, saturation=0.08)
r = rospy.Rate(rate)
while True:
if rospy.is_shutdown():
self.base_pub.publish(Twist())
return 'aborted'
if self.preempt_requested:
self.preempt_requested = False
self.base_pub.publish(Twist())
return 'preempted'
goal_mkr = create_base_marker(goal_ar_pose, 0, (0., 1., 0.))
self.mkr_pub.publish(goal_mkr)
if self.cur_ar_pose is not None:
# make sure we have a new observation
new_obs = self.ar_pose_updated
self.ar_pose_updated = False
# find the error between the AR tag and goal pose
print "self.cur_ar_pose", self.cur_ar_pose
cur_ar_pose_2d = homo_mat_from_2d(
*homo_mat_to_2d(self.cur_ar_pose))
print "cur_ar_pose_2d", cur_ar_pose_2d
ar_mkr = create_base_marker(cur_ar_pose_2d, 1, (1., 0., 0.))
self.mkr_pub.publish(ar_mkr)
ar_err = homo_mat_to_2d(cur_ar_pose_2d * goal_ar_pose**-1)
print "ar_err", ar_err
print "goal_ar_pose", goal_ar_pose
# filter this error using a Kalman filter
x_filt_err, x_filt_cov, x_unreli = kf_x.update(ar_err[0],
new_obs=new_obs)
y_filt_err, y_filt_cov, y_unreli = kf_y.update(ar_err[1],
new_obs=new_obs)
r_filt_err, r_filt_cov, r_unreli = kf_r.update(ar_err[2],
new_obs=new_obs)
if np.any(
np.array([x_unreli, y_unreli, r_unreli]) >
[lost_tag_thresh] * 3):
self.base_pub.publish(Twist())
return 'lost_tag'
print "Noise:", x_unreli, y_unreli, r_unreli
# TODO REMOVE
ma = Float32MultiArray()
ma.data = [
x_filt_err[0, 0], x_filt_err[1, 0], ar_err[0], x_unreli,
y_unreli, r_unreli
]
err_pub.publish(ma)
print "xerr"
print x_filt_err
print x_filt_cov
print "Cov", x_filt_cov[0, 0], y_filt_cov[0, 0], r_filt_cov[0,
0]
x_ctrl = pid_x.update_state(x_filt_err[0, 0])
y_ctrl = pid_y.update_state(y_filt_err[0, 0])
r_ctrl = pid_r.update_state(r_filt_err[0, 0])
base_twist = Twist()
base_twist.linear.x = x_ctrl
base_twist.linear.y = y_ctrl
base_twist.angular.z = r_ctrl
cur_filt_err = np.array(
[x_filt_err[0, 0], y_filt_err[0, 0], r_filt_err[0, 0]])
print "err", ar_err
print "Err filt", cur_filt_err
print "Twist:", base_twist
if np.all(np.fabs(cur_filt_err) < goal_error):
self.base_pub.publish(Twist())
return 'succeeded'
self.base_pub.publish(base_twist)
r.sleep()
def detect(input_image, save_img=True):
out, source, weights, view_img, save_txt, imgsz = \
opt.output, opt.source, opt.weights, opt.view_img, opt.save_txt, opt.img_size
global frame_num, model
global pubCentBlem, pubCentUnBlem
# Initialize
#device = torch_utils.select_device(opt.device)
#if os.path.exists(out):
# shutil.rmtree(out) # delete output folder
#os.makedirs(out) # make new output folder
half = device.type != 'cpu' # half precision only supported on CUDA
if half:
model.half() # to FP16
# Get names and colors
names = model.module.names if hasattr(model, 'module') else model.names
print("class names array ", names)
# names = ['blemished', 'unblemished', 'glove', 'belt', 'bin', 'head']
colors = [[random.randint(0, 255) for _ in range(3)]
for _ in range(len(names))]
# Run inference
t0 = time.time()
img = torch.zeros((1, 3, imgsz, imgsz), device=device) # init img
_ = model(img.half() if half else img
) if device.type != 'cpu' else None # run once
bounding_boxes_all_images = []
img0 = input_image.astype('float32')
#img0 = cv2.resize(img0, (640,480), interpolation = cv2.INTER_AREA)
img = letterbox(img0, new_shape=imgsz)[0]
# Convert
img = img[:, :, ::-1].transpose(2, 0, 1) # BGR to RGB, to 3x416x416
img = np.ascontiguousarray(img)
dataset = [("frame_num" + str(frame_num) + '.jpg', img, img0, None)]
for path, img, im0s, vid_cap in dataset:
img = torch.from_numpy(img).to(device)
img = img.half() if half else img.float() # uint8 to fp16/32
img /= 255.0 # 0 - 255 to 0.0 - 1.0
if img.ndimension() == 3:
img = img.unsqueeze(0)
# Inference
t1 = torch_utils.time_synchronized()
pred = model(img, augment=opt.augment)[0]
# pred = model(img)[0]
t2 = torch_utils.time_synchronized()
# Apply NMS
pred = non_max_suppression(pred,
opt.conf_thres,
opt.iou_thres,
classes=opt.classes,
agnostic=opt.agnostic_nms)
t3 = torch_utils.time_synchronized()
# needed specific to each image
Loc2arrCent = {}
arrCentBlem = []
arrCentUnBlem = []
Loc2Cls = {}
# Process detections
for i, det in enumerate(pred): # detections per image
p, s, im0 = path, '', im0s
save_path = str(Path(out) / Path(p).name)
s += '%gx%g ' % img.shape[2:] # print string
gn = torch.tensor(im0.shape)[[1, 0, 1,
0]] # normalization gain whwh
bounding_boxes = {}
if det is not None and len(det):
# Rescale boxes from img_size to im0 size
det[:, :4] = scale_coords(img.shape[2:], det[:, :4],
im0.shape).round()
# Print results
for c in det[:, -1].unique():
n = (det[:, -1] == c).sum() # detections per class
s += '%g %ss, ' % (n, names[int(c)]) # add to string
# Write results
minx = 5000
miny = 5000
maxx = 0
maxy = 0
box_num = 0
bounding_boxes = {}
for *xyxy, conf, cls in det:
box_num += 1
xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) /
gn).view(-1).tolist() # normalized xywh
#print("Box num:", box_num, " label: ",names[int(cls)], " xyxy: ", int(xyxy[0]), int(xyxy[1]), int(xyxy[2]), int(xyxy[3]), "xywh: ", float(xywh[0]), float(xywh[1]), float(xywh[2]), float(xywh[3]))
if (bounding_boxes.get(names[int(cls)], None) == None):
bounding_boxes[names[int(cls)]] = [[
int(xyxy[0]),
int(xyxy[1]),
abs(int(xyxy[2]) - int(xyxy[0])),
abs(int(xyxy[3]) - int(xyxy[1]))
]]
else:
bounding_boxes[names[int(cls)]].append([
int(xyxy[0]),
int(xyxy[1]),
abs(int(xyxy[2]) - int(xyxy[0])),
abs(int(xyxy[3]) - int(xyxy[1]))
])
tlx, tly, brx, bry = int(xyxy[0]), int(xyxy[1]), int(
xyxy[2]), int(xyxy[3])
if tlx < minx:
minx = tlx
if tly < miny:
miny = tly
if bry > maxy:
maxy = bry
if brx > maxx:
maxx = brx #crop_img = img[y:y+h, x:x+w]
if save_txt: # Write to file
xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) /
gn).view(-1).tolist() # normalized xywh
with open(save_path[:save_path.rfind('.')] + '.txt',
'a') as file:
file.write(('%g ' * 5 + '\n') %
(cls, *xywh)) # label format
if save_img or view_img: # Add bbox to image
label = '%s %.2f' % (names[int(cls)], conf)
# print("label input to plot_one_box ",label)
# plot_one_box(xyxy, im0, label=label, color=colors[int(cls)], line_thickness=3)
plot_one_box(xyxy,
im0,
label=label,
color=colors[int(cls)],
line_thickness=1)
# append onion centroid to list of centroids
if names[int(cls)] == 'blemished' or names[int(
cls)] == 'unblemished':
centx = (tlx + brx) / 2
centy = (tly + bry) / 2
if centx > 250:
# FOR CAMERA LOOKING FROM FRONT
# if center is on the belt
if names[int(cls)] == 'blemished':
Loc2Cls[centx] = 0
else:
Loc2Cls[centx] = 1
Loc2arrCent[centx] = (centx, centy)
if names[int(
cls
)] == 'blemished' and centx not in arrCentBlem:
arrCentBlem.append(centx)
arrCentBlem.append(centy)
else:
if centx not in arrCentUnBlem:
arrCentUnBlem.append(centx)
arrCentUnBlem.append(centy)
############################################### all bounding boxes for this frames: "bounding_boxes" ########################################################################################################
bounding_boxes_all_images.append(bounding_boxes)
print("Frame number = ", frame_num, "Bounding boxes: ",
bounding_boxes)
frame_num += 1
# Print time (inference + NMS)
print('%s Inference. (%.3fs)' % (s, t2 - t1))
print('%s NMS. (%.3fs)' % (s, t3 - t2))
#print('im0.shape before',im0.shape)
############################################### cropping
#im0 = im0[miny:maxy, minx:maxx]
#print('im0.shape after',im0.shape)
# Stream results
view_img = True
if view_img:
#cv2_imshow( im0)
not_showing = True
#cv2.imshow(p, im0)
if cv2.waitKey(1) == ord('q'): # q to quit
raise StopIteration
# Save results (image with detections)
save_img = True
if save_img: #'src/beginner_tutorials/scripts/yolov5/inference/output'
#save_path = '/home/psuresh/src/beginner_tutorials/scripts/yolov5/inference/output/frame1.jpg'
cv2.imwrite(save_path, im0) #if dataset.mode == 'images':
########################################################## comment this line to avoid saving images ########################################################################################################
# publish centroids for current image
print("publishing arrCentBlem ", arrCentBlem)
print("publishing arrCentUnBlem ", arrCentUnBlem)
pubCentBlem.publish(Float32MultiArray(data=arrCentBlem))
pubCentUnBlem.publish(Float32MultiArray(data=arrCentUnBlem))
# make array LocOrderedPreds
LocOrderedPreds = list(
OrderedDict(sorted(Loc2Cls.items(), key=lambda t: t[0])).values())
print("publishing LocOrderedPreds ", LocOrderedPreds)
pubLocOrderedPreds.publish(Int32MultiArray(data=LocOrderedPreds))
arrCentxyOrd = list(
OrderedDict(sorted(Loc2arrCent.items(),
key=lambda t: t[0])).values())
print("arrCentxyOrd ", arrCentxyOrd)
arrCentLocOrd = []
for i in range(len(arrCentxyOrd)):
arrCentLocOrd.append(arrCentxyOrd[i][0])
arrCentLocOrd.append(arrCentxyOrd[i][1])
print("publishing arrCentLocOrd ", arrCentLocOrd)
pubCentLocOrd.publish(Float32MultiArray(data=arrCentLocOrd))
#global img_processed
#img_processed = True
if save_txt or save_img:
print('Results saved to %s' % os.getcwd() + os.sep + out)
if platform == 'darwin': # MacOS
os.system('open ' + save_path)
print('Done. (%.3fs)' % (time.time() - t0))
return bounding_boxes_all_images
def __init__(self,
dim=2,
udp_ip='127.0.0.1',
udp_recv_port=8026,
udp_send_port=6001,
buffer_size=8192 * 4):
RosProcessingComm.__init__(self,
udp_ip=udp_ip,
udp_recv_port=udp_recv_port,
udp_send_port=udp_send_port,
buffer_size=buffer_size)
if rospy.has_param('framerate'):
self.frame_rate = rospy.get_param('framerate')
else:
self.frame_rate = 60.0
self.period = rospy.Duration(1.0 / self.frame_rate)
self.running = True
self.runningCV = threading.Condition()
self.rate = rospy.Rate(self.frame_rate)
self.lock = threading.Lock()
rospy.Subscriber('joy', Joy, self.joyCB)
self.human_control_pub = None
self.human_robot_pose_pub = None
self.initializePublishers()
self.dim = dim
self.num_goals = 2
assert (self.num_goals > 0)
self.goal_positions = npa([[0] * self.dim] * self.num_goals, dtype='f')
if rospy.has_param('max_cart_vel'):
self._max_cart_vel = np.array(rospy.get_param('max_cart_vel'))
else:
self._max_cart_vel = 0.25 * np.ones(self.dim)
rospy.logwarn(
'No rosparam for max_cart_vel found...Defaulting to max linear velocity of 50 cm/s and max rotational velocity of 50 degrees/s'
)
if rospy.has_param('width'):
self.width = rospy.get_param('width')
else:
self.width = 1200
if rospy.has_param('height'):
self.height = rospy.get_param('height')
else:
self.height = 900
self.user_vel = CartVelCmd()
_dim = [MultiArrayDimension()]
_dim[0].label = 'cartesian_velocity'
_dim[0].size = 2
_dim[0].stride = 2
self.user_vel.velocity.layout.dim = _dim
self.user_vel.velocity.data = np.zeros(self.dim)
self.user_vel.header.stamp = rospy.Time.now()
self.user_vel.header.frame_id = 'human_control'
self.human_robot_pose = np.zeros(self.dim) #UNUSED for the time being.
self.human_robot_pose_msg = Point()
self.getRobotPosition()
self.human_goal_pose = Float32MultiArray()
self.human_goal_pose.data = [
list(x) for x in list(self.goal_positions)
]
self.human_goal_pose_pub.publish(self.human_goal_pose)
self.data = CartVelCmd()
self._msg_dim = [MultiArrayDimension()]
self._msg_dim[0].label = 'cartesian_velocity'
self._msg_dim[0].size = 2
self._msg_dim[0].stride = 2
self.data.velocity.layout.dim = _dim
self.data.velocity.data = np.zeros(self.dim)
self.data.header.stamp = rospy.Time.now()
self.data.header.frame_id = 'human_control'
rospy.Service("point_robot_human_control/set_human_goals", GoalPoses,
self.set_human_goals)
self.send_thread = threading.Thread(target=self._publish_command,
args=(self.period, ))
self.send_thread.start()
rospy.loginfo("END OF CONSTRUCTOR - point_robot_human_control_node")
kiA = 25
kdA = 0.004
kpB = 0.42 #Antes 0.2
kiB = 25
kdB = 0.004
# Acumulacion de error para integrador
integrandA = []
integrandB = []
# Velocidades de referencia de las ruedas
desiredSpeedA = 0
desiredSpeedB = 0
# Velocidades actual de las ruedas
currentSpeedA = 0
currentSpeedB = 0
# Instancia para reutilizar de msnaje a topico de velocidad actual
messageCurrentSpeedPublisher = Float32MultiArray()
# Variable de saturacion maxima de ciclo util
dutyCycleSaturation = 70
# Ultimo error de la señal de velocidad
lastErrorA = 0
lastErrorB = 0
def setPins():
global pwmDriverA1, pwmDriverA2, pwmDriverB1, pwmDriverB2
# Configurandp estructura de pins de raspberry
GPIO.setmode(GPIO.BOARD)
# Configurando los pines de salida para el driver
GPIO.setup(pinDriverA1, GPIO.OUT)
GPIO.setup(pinDriverA2, GPIO.OUT)
GPIO.setup(pinDriverB1, GPIO.OUT)
def __init__(self, args):
pubs = {
"/homeostasis_motor": [
Float32MultiArray,
],
"/lpzros/x": [Float32MultiArray],
"/lpzros/xsi": [
Float32MultiArray,
],
"/lpzros/EE": [
Float32,
],
}
subs = {
"/tinyImu": [tinyIMU, self.cb_imu],
}
# gather arguments and transfer modes to integer representation
self.mode = LPZRos.modes[args.mode]
self.control_mode = LPZRos.control_modes[args.control_mode]
self.numtimesteps = args.numtimesteps
self.loop_time = args.looptime
self.automaticMode = args.automaticMode
self.verbose = args.verbose
self.save_pickle = args.save_pickle
self.period = args.period
smp_thread_ros.__init__(self,
loop_time=self.loop_time,
pubs=pubs,
subs=subs)
# initialize counters
self.cnt = 0
self.cb_imu_cnt = 0
############################################################
# model + meta params
# self.numsen_raw = 11 # 5 # 2
self.numsen = 6 # 5 # 2
self.nummot = 4 # introduce 2 ghost variables
self.imu_lin_acc_gain = 0 # 1e-3
self.imu_gyrosco_gain = 1 / 5000.
self.imu_orienta_gain = 0 # 1e-1
self.linear_gain = 0.0 # 1e-1
self.output_gain = 32000 # 5000 # velocitycontrol
self.msg_inputs = Float32MultiArray()
self.msg_xsi = Float32MultiArray()
self.msg_xsi.data = [0] * self.numsen
self.msg_motors = Float32MultiArray()
self.msg_motors.data = [0] * self.nummot
#self.msg_sensor_exp = Float64MultiArray()
# sphero lag is 4 timesteps
self.lag = 4 # 2 tested on puppy not so much influence
# buffer size accomodates causal minimum 1 + lag time steps
self.bufsize = 1 + self.lag # 2
self.creativity = args.creativity
# self.epsA = 0.1
# self.epsA = 0.02
self.epsA = args.epsA
# self.epsC = 0.001
#self.epsC = 0.01
#self.epsC = 0.9
self.epsC = args.epsC
# sampling scale for automaticMode
self.automaticModeScaleEpsA = 0.5
self.automaticModeScaleEpsC = 0.5
# initialize for logging
self.hz = 0
if self.automaticMode:
if self.control_mode == 2:
self.period = np.random.randint(1, 20) * 2
elif self.control_mode == 3:
self.period = 0
else:
self.epsA = np.random.exponential(
scale=self.automaticModeScaleEpsA, size=1)[0]
self.epsC = np.random.exponential(
scale=self.automaticModeScaleEpsC, size=1)[0]
print "EpsA:\t%f\nEpsC:\t%f\nCreativity:\t%f\nEpisodelength:\t%d\nLag:\t%d" % (
self.epsA, self.epsC, self.creativity, self.numtimesteps, self.lag)
############################################################
# forward model
self.A = np.random.uniform(-1e-1, 1e-1, (self.numsen, self.nummot))
self.b = np.zeros((self.numsen, 1))
print "initial A"
print self.A
# controller
self.C = np.random.uniform(-1e-1, 1e-1, (self.nummot, self.numsen))
print "initial C"
print self.C
self.h = np.zeros((self.nummot, 1))
self.g = np.tanh # sigmoidal activation function
self.g_ = dtanh # derivative of sigmoidal activation function
# state
self.x = np.ones((self.numsen, self.bufsize))
self.y = np.zeros((self.nummot, self.bufsize))
self.z = np.zeros((self.numsen, 1))
# auxiliary variables
self.L = np.zeros((self.numsen, self.nummot))
self.v_avg = np.zeros((self.numsen, 1))
self.xsi = np.zeros((self.numsen, 1))
self.xsiAvg = 0
self.xsiAvgSmooth = 0.01
self.imu = tinyIMU()
self.imu_vec = np.zeros((3 + 3, 1))
self.imu_smooth = 0.5 # coef
self.motorSmooth = 0.5
warnings.filterwarnings("error")
np.seterr(all='print')
self.exceptionCounter = 0
self.maxExceptionCounter = 10
if self.control_mode == 3:
self.epsA = 0
self.epsC = 0
self.sin_phase = 0
self.sin_speed = 0.1
# If something is changed in the variableDict style or content, change dataversion!
# Version History:
# 1: Initial Commit
# 2: xsi was not saved correctly before.
# 3: lag and automaticMode + scales included
# 4: exceptionCounter added
# 5: exceptionCounter working :) forgot to increase versionnumber and wasn't working good
# 6: added file id (when there are multiple results with the same eps and c) and filename
# 7: added weight_in_body should be used to indicate if the battery is built in or not. For all measurements before, it was true
# 8: added control_mode variable 1 = position control, 2 = velocity control, 3= step, also period for step period
# 9: added hz of swinging motion in new control mode 4 = sin_sweep
self.variableDict = {
"dataversion": 9,
"control_mode": self.control_mode,
"timesteps": self.numtimesteps,
"looptime": self.loop_time,
"startTime": time.time(),
"endTime": 0,
"numsen": self.numsen,
"nummot": self.nummot,
"epsA": self.epsA,
"epsC": self.epsC,
"creativity": self.creativity,
"imuSmooth": self.imu_smooth,
"motorSmooth": self.motorSmooth,
"lag": self.lag,
"period": self.period,
"hz": self.hz,
"automaticMode": self.automaticMode,
"automaticModeScaleEpsA": self.automaticModeScaleEpsA,
"automaticModeScaleEpsC": self.automaticModeScaleEpsC,
"exceptionCounter": 0,
"id": 0,
"filename": 0,
"weight_in_body": False,
"C": np.zeros((self.numtimesteps, ) + self.C.shape),
"A": np.zeros((self.numtimesteps, ) + self.A.shape),
"h": np.zeros((self.numtimesteps, ) + self.h.shape),
"b": np.zeros((self.numtimesteps, ) + self.b.shape),
"x": np.zeros((self.numtimesteps, ) + self.x.shape),
"y": np.zeros((self.numtimesteps, ) + self.y.shape),
"xsi": np.zeros((self.numtimesteps, ) + self.xsi.shape),
}
def draw(self):
self.screen.fill(pygame.Color('grey'))
xAndYArray = []
prev_distance = 0
max_point_difference = 0
noise_index_array = []
temp_index_array = []
angle_started = False
temp_angle = self.angle_min
for i in range(len(self.ranges)):
#if i in noise_index_array:
#color = pygame.Color('red')
#else:
#color = pygame.Color('blue')
self.current_angle = i*self.angle_increment + self.angle_min
if(not math.isnan(self.ranges[i])):
x = -int(self.ranges[i]*math.sin(self.current_angle)*self.scaling)
y = -int(self.ranges[i]*math.cos(self.current_angle)*self.scaling)
if(abs(x) > + self.tolerance and abs(y) > self.tolerance):
xAndYArray.append([x + self.model.width/2, y + self.model.height/2])
pygame.draw.circle(self.screen, pygame.Color('blue'), (x + self.model.width/2,y + self.model.height/2), 2)
#print self.heading_x
if(len(xAndYArray) > 0):
rdp_array = dpa.rdp(xAndYArray, 2)
#print rdp_array
line_array = []
for i in range(len(rdp_array)):
#print rdp_array[i]
x = rdp_array[i][0]
y = rdp_array[i][1]
pygame.draw.circle(self.screen, pygame.Color('green'), (x,y), 2)
for j in range(i, len(rdp_array)):
x1 = rdp_array[i][0]
y1 = rdp_array[i][1]
x2 = rdp_array[j][0]
y2 = rdp_array[j][1]
length = dpa.distance((x1, y1),(x2, y2))
if((x2 -x1) != 0 and length < (10*self.scaling)and length > (.5*self.scaling)):
slope = (y2-y1)/(x2-x1)
line_array.append(Line(x1, y1, x2, y2, self.get_angle((x1, y1), (x2, y2))))
#for i in renage(len(slope_array)):
for i, line in enumerate(line_array):
#line.reset_dots()
for j, point in enumerate(xAndYArray):
distance_to_line = dpa.point_line_distance((point[0], point[1]), (line.x1, line.y1), (line.x2, line.y2))
distance_to_start = dpa.distance((point[0], point[1]),(line.x1, line.y1))
if (distance_to_line < .14*self.scaling and distance_to_line > 0):
#print distance_to_line
line.number_of_dots += 1
line.total_distance_to_start += distance_to_start
#print line.number_of_dots
line_distance = dpa.distance((line.x1, line.y1), (line.x2, line.y2))
line.line_distance = line_distance
if(line.number_of_dots > 5 and line_distance/line.number_of_dots < 1.5):
#(line.number_of_dots > 30 and line.total_distance_to_start/line.number_of_dots < 1.5*self.scaling):
pygame.draw.line(self.screen, pygame.Color('red'), (line.x1, line.y1), (line.x2, line.y2), 2)
line.is_good_line = True
number_of_lines = 0
total = 0
for i, line, in enumerate(line_array):
if line.is_good_line:
if line.number_of_dots > 0:
angle = line.angle
while (angle < 0):
angle += math.pi
while (angle > math.pi):
angle -= math.pi
total += angle*line.line_distance#/line.number_of_dots
#print angle*180.0/math.pi, line.line_distance
number_of_lines += 1*line.line_distance#/line.number_of_dots
#print angle
#print 'start'
average = 0
if (number_of_lines != 0):
average = total/number_of_lines
point3 = ((self.center[0] - math.cos(average)*100), (self.center[1] - math.sin(average)*100))
pygame.draw.line(self.screen, pygame.Color('green'), self.center, point3, 5)
if (average != 0):
if(self.cycle > 5):
total_average = self.total_cycle/6
if total_average < 0:
total_average += math.pi
self.turn_direction = total_average - math.pi/2
self.current_go_heading = self.turn_direction - self.heading
self.turn_array = [0]*11
#print self.turn_direction*7/(math.pi/6)
index = int(self.turn_direction*7/(math.pi/6))
if index < -4:
index = -5
if index > 4:
index = 5
self.turn_array[5 + index] = .01
msg = Float32MultiArray()
self.vel_array = [0]*11
total_array = self.vel_array
total_array.extend(self.turn_array)
msg.data = total_array
self.pub.publish(msg)
print self.turn_array
self.heading_point = ((self.center[0] - math.cos(total_average)*100), (self.center[1] - math.sin(total_average)*100))
self.cycle = 0
self.total_cycle = 0
else:
self.cycle += 1
self.total_cycle += average
pygame.draw.line(self.screen, pygame.Color('black'), self.center, self.heading_point, 5)
pygame.display.update()
#!/usr/bin/env python
import rospy
from dynamic_reconfigure.server import Server
from arm_mobility.cfg import armGainConfig
from std_msgs.msg import Float32MultiArray
outMsg = Float32MultiArray()
def callback(config, level):
rospy.loginfo("""Reconfigure Request: Kp {Kp} Ki {Ki} Kd {Kd} Kii {Kii} """.format(**config))
data =[config.Kp,config.Ki, config.Kd, config.Kii]
rospy.loginfo("Gains Changed")
rospy.loginfo(data)
outMsg.data = data;
pub.publish(outMsg)
return config
if __name__ == "__main__":
rospy.init_node("arm_dyn_server", anonymous = True)
pub = rospy.Publisher('arm_conf_mssg', Float32MultiArray, queue_size=10)
srv = Server(armGainConfig, callback)
rospy.spin()
global open_paths
open_paths = paths.data
def scan_callback(msg):
global scan
scan = msg.data
def handle_path_data(req): # [self.aim_steer, self.aim_dist, self.steer_ang, self.speed]
global open_window
try:
window = get_boundaries(req.data[0])
except IndexError:
window = (0,0,0,0,0,0)
open_window.data = window
pub.publish(open_window)
return PathDataResponse(window)
if __name__ == '__main__':
open_paths = []
scan = []
edge = 12 # minimum sudden change in distance to be considered an edge
open_window = Float32MultiArray()
rospy.init_node('window_publisher')
rospy.Subscriber('/open_paths', Float32MultiArray, open_paths_callback)
rospy.Subscriber('/scan_transformed', Float32MultiArray, scan_callback)
pub = rospy.Publisher('/open_window', Float32MultiArray, queue_size=1)
s = rospy.Service('open_window', PathData, handle_path_data)
rospy.spin()
epsilon_decay = 0.99
epsilon_min = 0.04
rospy.init_node('ddpg_classes')
pub_result = rospy.Publisher('result', Float32MultiArray, queue_size=5)
pub_get_action = rospy.Publisher('get_action',
Float32MultiArray,
queue_size=5)
train_indicator = False
sess = tf.Session()
backend.set_session(sess)
actor = Actor(sess, state_dim, TAU, LRA)
critic = Critic(sess, state_dim, TAU, LRC)
result = Float32MultiArray()
get_action = Float32MultiArray()
past_action = np.zeros((1, 2))
if load_weight == True:
print("Now we load the weight")
try:
actor.model.load_weights("actormodel.h5")
critic.model.load_weights("criticmodel.h5")
actor.target_model.load_weights("actormodel.h5")
critic.target_model.load_weights("criticmodel.h5")
print("Weight load successfully")
except:
print("Cannot find the weight")
env = Env(2)
def array_publish(data): # fonksiyon içinde fonksiyon tanımlandı
array = Float32MultiArray(data=data)
cmd.publish(array) # dizi elemanları tek tek paylaşılır
def __init__(self, conf={}):
"""SpheroSys.__init__
Arguments:
- conf: configuration dictionary
-- mass: point _mass_
-- sysdim: dimension of system, usually 1,2, or 3D
-- statedim: 1d pointmass has 3 variables (pos, vel, acc) in this model, so sysdim * 3
-- dt: integration time step
-- x0: initial state
-- order: NOT IMPLEMENT (control mode of the system, order = 0 kinematic, 1 velocity, 2 force)
"""
SMPSys.__init__(self, conf)
# state is (pos, vel, acc).T
# self.state_dim
if not hasattr(self, 'x0'):
self.x0 = np.zeros((self.dim_s_proprio, 1))
self.x = self.x0.copy()
self.cnt = 0
if not self.ros:
import sys
print(
"ROS not configured but this robot (%s) requires ROS, exiting"
% (self.__class__.__name__))
sys.exit(1)
# self.pubs = {
# 'motors': rospy.Publisher(
# "/lpzbarrel/motors", Float64MultiArray, queue_size = 2),
# 'sensors': rospy.Publisher(
# "/lpzbarrel/x", Float32MultiArray, queue_size = 2)
# }
# self.subs = {
# 'sensors': rospy.Subscriber("/lpzbarrel/sensors", Float64MultiArray, self.cb_sensors),
# }
self.pubs = {
'_cmd_vel':
rospy.Publisher("/cmd_vel", Twist, queue_size=2),
'_cmd_vel_raw':
rospy.Publisher("/cmd_vel_raw", Twist, queue_size=2),
'_cmd_raw_motors':
rospy.Publisher("/cmd_raw_motors", Float32MultiArray,
queue_size=2),
'_set_color':
rospy.Publisher("/set_color", ColorRGBA, queue_size=2),
'_lpzros_x':
rospy.Publisher("/lpzros/x", Float32MultiArray, queue_size=2),
'_set_stab':
rospy.Publisher('disable_stabilization', Bool, queue_size=2),
}
# self.cb = {
# "/imu": get_cb_dict(self.cb_imu),
# "/odom": get_cb_dict(self.cb_odom),
# }
self.subs = {
'imu': rospy.Subscriber("/imu", Imu, self.cb_imu),
'odom': rospy.Subscriber("/odom", Odometry, self.cb_odom),
}
# timing
self.rate = rospy.Rate(1 / self.dt)
self.cb_imu_cnt = 0
self.cb_odom_cnt = 0
# custom
self.numsen_raw = 10 # 8 # 5 # 2
self.numsen = 10 # 8 # 5 # 2
self.imu = Imu()
self.odom = Odometry()
# sphero color
self.color = ColorRGBA()
self.motors = Twist()
self.raw_motors = Float32MultiArray()
self.raw_motors.data = [0.0 for i in range(4)]
self.msg_inputs = Float32MultiArray()
self.msg_motors = Float64MultiArray()
self.msg_sensor_exp = Float64MultiArray()
self.imu_vec = np.zeros((3 + 3 + 3, 1))
self.imu_smooth = 0.8 # coef
self.imu_lin_acc_gain = 0 # 1e-1
self.imu_gyrosco_gain = 1e-1
self.imu_orienta_gain = 0 # 1e-1
self.linear_gain = 1.0 # 1e-1
self.pos_gain = 0 # 1e-2
self.angular_gain = 360.0 # 1e-1
self.output_gain = 255 # 120 # 120
# sphero lag is 4 timesteps
self.lag = 1 # 2
# enable stabilization
"""$ rostopic pub /disable_stabilization std_msgs/Bool False"""
stab = Bool()
stab.data = False
print("stab", stab, stab.data)
for i in range(5):
self.pubs['_set_stab'].publish(stab)
def callback(data):
list_obj = listobj()
list_obj.single_obj_info = []
brocili_list = broclist()
brocili_list.broc_uv_list = []
temp_list_broc = []
global temp_single_obj
#print(data.objects_vector[0].id)
# print(data.objects_vector[0].classname)
# print(data.objects_vector[0].score)
#print("---"*20)
single_obj = sglobj()
#print("single_obj",single_obj)
single_obj.element_data_temp = []
single_obj.classname = []
single_obj.score = []
get_msg = data.objects_vector[0]
bridge = CvBridge()
cv_image = bridge.imgmsg_to_cv2(data.rgb_img, "bgr8")
cv_depth_image = bridge.imgmsg_to_cv2(data.depth_img, "passthrough")
rgb_img_to_pos = cv_image.copy()
depth_img_to_pos = cv_depth_image.copy()
plate_score = [0, 0]
pan_score = [0]
vegetablebowl_score = [0]
broccoli_score = [0, 0, 0]
souppothandle_score = [0]
panhandle_score = [0]
beef_score = [0]
nethandle_score = [0]
seasoningbottle_score = [0, 0]
seasoningbowl_score = [0]
for i in range(len(get_msg.id)):
if (get_msg.classname[i] == "pan"):
pan_score.append(get_msg.score[i])
if (get_msg.classname[i] == "beef"):
beef_score.append(get_msg.score[i])
if (get_msg.classname[i] == "plate"):
plate_score.append(get_msg.score[i])
if (get_msg.classname[i] == "vegetablebowl"):
vegetablebowl_score.append(get_msg.score[i])
if (get_msg.classname[i] == "broccoli"):
broccoli_score.append(get_msg.score[i])
if (get_msg.classname[i] == "souppothandle"):
souppothandle_score.append(get_msg.score[i])
if (get_msg.classname[i] == "panhandle"):
panhandle_score.append(get_msg.score[i])
if (get_msg.classname[i] == "seasoningbowl"):
beef_score.append(get_msg.score[i])
if (get_msg.classname[i] == "nethandle"):
nethandle_score.append(get_msg.score[i])
if (get_msg.classname[i] == "seasoningbottle"):
seasoningbottle_score.append(get_msg.score[i])
plate_score = sorted(plate_score, reverse=True)
pan_score = sorted(pan_score, reverse=True)
vegetablebowl_score = sorted(vegetablebowl_score, reverse=True)
broccoli_score = sorted(broccoli_score, reverse=True)
souppothandle_score = sorted(souppothandle_score, reverse=True)
panhandle_score = sorted(panhandle_score, reverse=True)
beef_score = sorted(beef_score, reverse=True)
nethandle_score = sorted(nethandle_score, reverse=True)
seasoningbottle_score = sorted(seasoningbottle_score, reverse=True)
seasoningbowl_score = sorted(seasoningbowl_score, reverse=True)
det_bar_pos = detectionbar.model_detection(cv_image)
bro_list = broccolidetection.broccoli_detection(cv_image)
global temp_list_broc
if len(bro_list) > 0:
brocili_list.broc_uv_list = []
brocili_list.broc_uv_list = bro_list
temp_list_broc = bro_list
if len(bro_list) <= 0:
brocili_list.broc_uv_list = temp_list_broc
#print("wo cao :",temp_list_broc)
#print("xi lan hua: ",brocili_list.broc_uv_list)
'''
print("plate score : ",plate_score)
print("pan score : ",pan_score)
print("vegetablebowl score : ",vegetablebowl_score)
print("broccoli score : ",broccoli_score)
print("souppothandle score : ",souppothandle_score)
print("panhandle score : ",panhandle_score)
print("beef score : ",beef_score)
print("nethandle score : ",nethandle_score)
print("seasoningbottle score : ",seasoningbottle_score)
print("seasoningbowl score : ",seasoningbowl_score)
'''
single_obj.element_data_temp = []
single_obj.classname = []
single_obj.score = []
for i in range(len(get_msg.id)):
element_data = element_info()
element_data.id_info = []
mask_area = get_msg.roi[i].height * get_msg.roi[i].width
if mask_area > 0 and get_msg.score[i] > 0.19:
mask_image = bridge.imgmsg_to_cv2(get_msg.masks[i], "passthrough")
if get_msg.classname[i] == "plate":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > plate1[0] and center_x < plate1[
1] and center_y > plate1[2] and center_y < plate1[3]:
temp_list = []
temp_list = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list != []:
#element_data.id_info = []
element_data.id_info = temp_list
single_obj.element_data_temp.append(element_data)
#print("aaa:",single_obj.element_data_temp)
single_obj.score.append(get_msg.score[i])
single_obj.classname.append("plate1")
if get_msg.classname[i] == "plate":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > plate2[0] and center_x < plate2[
1] and center_y > plate2[2] and center_y < plate2[3]:
temp_list1 = []
temp_list1 = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list1 != []:
#element_data.id_info = []
element_data.id_info = temp_list1
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
single_obj.classname.append("plate2")
if get_msg.classname[i] == "plate":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > plate3[0] and center_x < plate3[
1] and center_y > plate3[2] and center_y < plate3[3]:
temp_list2 = []
temp_list2 = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list2 != []:
element_data.id_info = temp_list2
#print("wo-2-cia",temp_list)
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
single_obj.classname.append("plate3")
if get_msg.classname[i] == "beef" and get_msg.score[
i] == beef_score[0]:
temp_list3 = []
temp_list3 = prcess_contours(
cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset,
get_msg.roi[i].height, get_msg.roi[i].width,
get_msg.score[i])
if temp_list3 != []:
element_data.id_info = temp_list3
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "pan" and get_msg.score[i] == pan_score[
0]:
temp_list4 = []
temp_list4 = prcess_contours(
cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset,
get_msg.roi[i].height, get_msg.roi[i].width,
get_msg.score[i])
if temp_list4 != []:
element_data.id_info = temp_list4
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[0]:
temp_list5 =[]
temp_list = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list5!=[]:
element_data.id_info = temp_list5
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[1]:
temp_list6 = []
temp_list6 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list6!=[]:
element_data.id_info = temp_list6
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[2]:
temp_list7 = []
temp_list7 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list7!=[]:
element_data.id_info = temp_list7
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "souppothandle" and get_msg.score[i] == souppothandle_score[0]:
temp_list8 = []
temp_list8 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list8!=[]:
element_data.id_info = temp_list8
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
'''
if get_msg.classname[i] == "nethandle" and get_msg.score[i] == nethandle_score[0]:
temp_list9 = []
temp_list9 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list9!=[]:
element_data.id_info = temp_list9
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "vegetablebowl" and get_msg.score[i] == vegetablebowl_score[0]:
temp_list10 = []
temp_list10 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list10!=[]:
element_data.id_info = temp_list10
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
if get_msg.classname[i] == "seasoningbowl" and get_msg.score[
i] == seasoningbowl_score[0]:
temp_list11 = []
temp_list11 = prcess_contours(
cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset,
get_msg.roi[i].height, get_msg.roi[i].width,
get_msg.score[i])
if temp_list11 != []:
element_data.id_info = temp_list11
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "seasoningbottle":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > bottle1[0] and center_x < bottle1[
1] and center_y > bottle1[2] and center_y < bottle1[3]:
temp_list12 = []
temp_list12 = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list12 != []:
single_obj.classname.append("seasoningbottle1")
element_data.id_info = temp_list12
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
if get_msg.classname[i] == "seasoningbottle":
center_x = int(get_msg.roi[i].x_offset +
get_msg.roi[i].height / 2) # rect area center
center_y = int(get_msg.roi[i].y_offset +
get_msg.roi[i].width / 2) # rect area center
if center_x > bottle2[0] and center_x < bottle2[
1] and center_y > bottle2[2] and center_y < bottle2[3]:
temp_list13 = []
temp_list13 = prcess_contours(
cv_image, get_msg.classname[i], mask_image,
get_msg.id[i], get_msg.roi[i].x_offset,
get_msg.roi[i].y_offset, get_msg.roi[i].height,
get_msg.roi[i].width, get_msg.score[i])
if temp_list13 != []:
single_obj.classname.append("seasoningbottle2")
element_data.id_info = temp_list13
single_obj.element_data_temp.append(element_data)
#single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
if get_msg.classname[i] == "panhandle" and get_msg.score[i] == panhandle_score[0]:
temp_list14 = []
temp_list14 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i],
get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i])
if temp_list14!=[]:
element_data.id_info = temp_list14
single_obj.element_data_temp.append(element_data)
single_obj.classname.append(get_msg.classname[i])
single_obj.score.append(get_msg.score[i])
'''
#print("88888",single_obj)
if len(single_obj.score) > 0:
#print("1111:",single_obj)
temp_single_obj = single_obj
#print("****",temp_single_obj)
list_obj.single_obj_info.append(single_obj)
else:
list_obj.single_obj_info.append(temp_single_obj)
#print("1111:",list_obj.single_obj_info)
list_obj.header.stamp = data.header.stamp
list_obj.header.frame_id = data.header.frame_id
print("---" * 20)
print("--ww--", list_obj.single_obj_info)
list_obj.rgb_img_to_pos = bridge.cv2_to_imgmsg(rgb_img_to_pos,
encoding="bgr8")
list_obj.depth_img_to_pos = bridge.cv2_to_imgmsg(depth_img_to_pos,
"passthrough")
temp_det_bar_pos = Float32MultiArray(data=det_bar_pos)
#print("1111:",list_obj)
list_obj_pub = rospy.Publisher("process_uv/listobjs",
listobj,
queue_size=1)
temp_det_bar_pos_pub = rospy.Publisher("process_uv/bar_pos",
Float32MultiArray,
queue_size=1)
list_broc_pub = rospy.Publisher("process_uv/listbroc",
broclist,
queue_size=1)
list_broc_pub.publish(brocili_list)
list_obj_pub.publish(list_obj)
temp_det_bar_pos_pub.publish(temp_det_bar_pos)
cv2.rectangle(cv_image, (plate2[0], plate2[2]),
(plate2[0] + (plate2[1] - plate2[0]), plate2[2] +
(plate2[3] - plate2[2])), (0, 0, 255), 2)
cv2.rectangle(cv_image, (plate1[0], plate1[2]),
(plate1[0] + (plate1[1] - plate1[0]), plate1[2] +
(plate1[3] - plate1[2])), (0, 0, 255), 2)
cv2.rectangle(cv_image, (plate3[0], plate3[2]),
(plate3[0] + (plate3[1] - plate3[0]), plate3[2] +
(plate3[3] - plate3[2])), (0, 0, 255), 2)
#cv2.rectangle(cv_image,(bottle1[0], bottle1[2]),(bottle1[0]+(bottle1[1]-bottle1[0])/2, bottle1[2]+(bottle1[3]-bottle1[2])/2),(0,0,255),2)
#cv2.rectangle(cv_image,(bottle2[0], bottle2[2]),(bottle2[0]+(bottle2[1]-bottle2[0])/2, bottle2[2]+(bottle2[3]-bottle2[2])/2),(0,0,255),2)
cv2.imshow("img2", cv_image)
cv2.waitKey(1)
def RadarPub(data):
Radar_Dist = Float32MultiArray()
Radar_Dist.data = [0, 0, 0]
if (data.id != 1343):
a = UInt8MultiArray
a = [0, 0, 0, 0, 0, 0, 0, 0] # data bytes
print("data", data)
for i in range(8):
a[i] = ord(data.data[i])
print("a", a)
byte_L5 = '{0:08b}'.format(a[5])
byte_H6 = '{0:08b}'.format(a[6])
Radial_angle = byte_H6[3:8] + byte_L5[0:6]
Radial_angle = int(Radial_angle, 2)
Radial_angle = Radial_angle * 0.1 # scaled value
if (Radial_angle > 102.3):
Radial_angle = Radial_angle - 204.8
else:
Radial_angle = Radial_angle
angle = round(Radial_angle, 2)
print "Scaled_Radial_angle = ", angle, "Degree"
byte_L0 = '{0:08b}'.format(a[0])
byte_H1 = '{0:08b}'.format(a[1])
Radial_range = byte_H1[1:8] + byte_L0
Radial_range = int(Radial_range, 2)
Radial_range = Radial_range * 0.01 # scaled value
R_range = (round(Radial_range, 2))
print "Scaled_Radial_range = ", R_range, "m"
if data.id < 1343:
if (angle >= -15 and angle <= 15): # Center Object distance
print("Center", angle)
string_range = str(R_range)
All_Rdis_Center.append(R_range)
print("Centre dis", min(All_Rdis_Center))
elif (angle < -15 and angle >= -45): # Left Object distance
print(" Left ", angle)
string_range = str(R_range)
All_Rdis_Left.append(R_range)
print("Left dis", min(All_Rdis_Left))
elif (angle > 15 and angle <= 45): # Right Object distance
print(" Right ", angle)
string_range = str(R_range)
All_Rdis_Right.append(R_range)
print("Right dis", min(All_Rdis_Right))
elif data.id >= 1343:
Rdis_Center = min(All_Rdis_Center)
Rdis_Left = min(All_Rdis_Left)
Rdis_Right = min(All_Rdis_Right)
Radar_Dist = [Rdis_Left, Rdis_Center, Rdis_Right]
print(Radar_Dist)
global cond
cond = String
'''if Rdis_Center <= 2 and Rdis_Left <= 2 and Rdis_Right <= 2:
cond = "A"
print("stop")
if Rdis_Center <= 2:
cond = "A"
print("stop")
elif Rdis_Center <= 2 and Rdis_Left <= 2 and Rdis_Right >= 2:
cond = "B"
print("go right")
elif Rdis_Center <= 2 and Rdis_Left >= 2 and Rdis_Right <= 2:
cond = "C"
print("go left")
elif Rdis_Center <= 2 and Rdis_Left >= 2 and Rdis_Right >= 2:
if Rdis_Left > Rdis_Right:
cond = "C"
print("go left")
else:
print("go right")
cond = "B"'''
if Rdis_Center <= 1.5:
cond = "A"
print("Stop")
elif Rdis_Center <= 1.5 and Rdis_Left <= 1.5 and Rdis_Right <= 1.5:
cond = "B"
print("stop")
elif Rdis_Left <= 1.5:
cond = "C"
print("go right")
elif Rdis_Right <= 1.5:
cond = "D"
print("go left")
else:
cond = "E"
print("goes straight")
'''
#print (" Radar_Center", Rdis_Center)
#print (" Radar_Left", Rdis_Left)
#print (" Radar_Right", Rdis_Right)
print(Radar_Dist)
#key = input() '''
pub = rospy.Publisher("Radar", String, queue_size=10)
pub.publish(cond)
#rospy.loginfo(Radar_Dist)
del All_Rdis_Center[:]
del All_Rdis_Left[:]
del All_Rdis_Right[:]
# get path of pkg
rospy.init_node("map_setup")
# load parameters
max_prm_size = rospy.get_param('~max_prm_size', 1000)
# wait for services
rospy.wait_for_service('/rosplan_roadmap_server/create_prm')
wp_pub = rospy.Publisher('/uav01/add_waypoint',
Float32MultiArray,
queue_size=100)
# generate dense PRM
rospy.loginfo("KCL: (%s) Creating PRM of size %i" %
(rospy.get_name(), max_prm_size))
prm = rospy.ServiceProxy('/rosplan_roadmap_server/create_prm', CreatePRM)
if not prm(max_prm_size, 2, 3, 4, 50, 100000):
rospy.logerr("KCL: (%s) No PRM was made" % rospy.get_name())
# load ground_wps from parameter
ground_wps = rospy.get_param("/ground_wp")
# add each as a new waypoint to the PMR (usin sensing interface)
for wp in ground_wps:
if type(ground_wps[wp]) is list:
rospy.loginfo("KCL: (%s) Adding wp: %s" % (rospy.get_name(), wp))
wpmsg = Float32MultiArray()
for coord in ground_wps[wp]:
wpmsg.data.append(coord)
wp_pub.publish(wpmsg)
from Environment import Environment
import pdb
EPISODES = 6000
if __name__ == '__main__':
rospy.init_node('turtlebot2i_deep_qlearning')
publish_result = rospy.Publisher('result', Float32MultiArray, queue_size=5)
publish_action = rospy.Publisher('get_action',
Float32MultiArray,
queue_size=5)
# store the action taken and the result
action_data = Float32MultiArray()
result = Float32MultiArray()
state_space_size = 28
num_of_actions = 5
parameter_dictionary = None
environment = Environment(num_of_actions)
neural_network = NeuralNetwork(state_space_size, num_of_actions)
scores, episodes = [], []
global_step = 0
start_time = time.time()
for episode in range(neural_network.load_episode + 1, EPISODES):
def move_closer(self):
rospy.loginfo("[Decon] Starting to move...")
request = Float32MultiArray()
request.data = self.move_position
self.move_closer_command_topic.publish(request)
def callback(scan):
#print(scan)
# if scan.intensities[0] > 0:
# intensities 360 length 0 or 47
# scan_time 7.96710082795e-05
# ranges tuple float32
# angle_min -3.12413907051
# 3.14159274101
# angle_increment 0.0157
global ranges_filtered
global num_accumulated
ranges_np = np.array(scan.ranges)
ranges_np[ranges_np == inf] = 0
ranges_np[ranges_np > 1] = 0
ranges_angle = 80
ranges_np[ranges_angle + 1:360 - ranges_angle] = 0
num_accumulated = 8
center = []
center_index = 0
if 1 > 0:
temp = ranges_filtered[:, 1:num_accumulated]
ranges_filtered[:, 0:num_accumulated - 1] = temp
ranges_filtered[:, num_accumulated - 1] = ranges_np
ranges_median = np.median(ranges_filtered, axis=1)
ranges_scan = np.zeros(2 * ranges_angle)
ranges_scan[0:ranges_angle] = ranges_median[360 - ranges_angle:360]
ranges_scan[ranges_angle:2 *
ranges_angle] = ranges_median[0:ranges_angle]
ranges_scan = ranges_scan[::-1]
i = 0
index = 0
center_index = 0
center = []
while i < 2 * ranges_angle:
if ranges_scan[i] > 0:
index = int(np.ceil(0.2 / ranges_scan[i] / 0.0175)) + 2
flag = 0
for j in range(index):
if i + j > 2 * ranges_angle - 1:
break
if abs(ranges_scan[i] -
ranges_scan[i + j]) < 0.25 and ranges_scan[i +
j] > 0:
flag = j
if flag > 0 and flag > index / 2:
ranges_scan[i:i + index] = np.sqrt(ranges_scan[i]**2 -
0.1**2)
center_ranges = np.sqrt(ranges_scan[i]**2 - 0.1**2)
center_index = np.floor(i + flag / 2)
print(i)
print(index)
center_index = np.mod((360 + center_index - ranges_angle),
360).astype('int')
center = ranges_index_to_point(center_ranges, center_index)
i = i + index
else:
i += 1
else:
i += 1
range_median = np.zeros(360)
ranges_scan = ranges_scan[::-1]
ranges_median[360 - ranges_angle:360] = ranges_scan[0:ranges_angle]
ranges_median[0:ranges_angle] = ranges_scan[ranges_angle:2 *
ranges_angle]
location = Float32MultiArray()
location.data = center
print(center)
print(center_index)
publisher_single_location(location)
scan_median = scan
scan_median.ranges = tuple(ranges_median)
publisher(scan_median)
def pub_test(self):
print('Printing test')
test = Float32MultiArray()
self.pubPose.publish(test)
import tornado.websocket
import tornado.ioloop
import tornado.web
# FLAG for fully manual control (TRUE) or shared control (FALSE)
#Tonado server port
try:
os.nice(-10)
except PermissionError:
# Need to run via sudo for high priority
rospy.logerr("Cannot set niceness for the process...")
rospy.logerr("Run the script as sudo...")
pub_remote = rospy.Publisher('qolo/user_commands',
Float32MultiArray,
queue_size=1)
data_remote = Float32MultiArray()
### ---------- GLOBAL VARIABLES ---------- ####
PORT = 8080
Max_V = 0.4
Max_W = 0.4
level_relations = {
# 'debug':logging.DEBUG,
'info': logging.INFO,
# 'warning':logging.WARNING,
# 'error':logging.ERROR,
# 'crit':logging.CRITICAL
}
# Tornado Folder Paths
#!/usr/bin/env python
# -*- coding: utf-8 -*-
#
# FileName: sample
# CreatedDate: 2019-06-05 18:25:22
#
import rospy
from std_msgs.msg import Float32MultiArray
if __name__ == "__main__":
rospy.init_node("sample", anonymous=True)
pub = rospy.Publisher("/maxon_bringup/all_position", Float32MultiArray, queue_size=1)
pub.publish(Float32MultiArray(data=[10000, -20000]))
def joint_states_received(self, msg):
self.encoders_pub.publish(
Float32MultiArray(data=[
msg.position[0] * self.wheel_radius, msg.position[1] *
self.wheel_radius
]))
