def callback(rightfront_sub, leftfront_sub, value, angle):
msg = vect_msg()
#TODO define here a behavior according to the contact value
# angle = (forceL - forceR) * 180/1023
# force = (forceL+forceR)/2
msg.value = value
msg.angle = angle
if not rightfront_sub.states:
pass
else:
rightfront_force = rightfront_sub.states[0].total_wrench.force.x
rospy.loginfo('Ive been toched from the right F=%s\n',
rightfront_force)
# Fill vector message
angle = -90
value = -1
right_now = rospy.Time.now()
seconds = rospy.Duration(2)
endtime = right_now + seconds
msg.header.stamp = rospy.Time.now()
msg.value = value
msg.angle = angle
rospy.loginfo('Force vector data sent')
pub = rospy.Publisher('force_vect', vect_msg, queue_size=10)
while rospy.Time.now() < endtime:
pub.publish(msg)
if not leftfront_sub.states:
pass
else:
leftfront_force = leftfront_sub.states[0].total_wrench.force.x
rospy.loginfo('Ive been toched from the left F=%s\n', leftfront_force)
# Fill vector message
angle = 90
value = -1
right_now = rospy.Time.now()
seconds = rospy.Duration(2)
endtime = right_now + seconds
msg.header.stamp = rospy.Time.now()
msg.value = value
msg.angle = angle
rospy.loginfo('Force vector data sent')
pub = rospy.Publisher('force_vect', vect_msg, queue_size=10)
while rospy.Time.now() < endtime:
pub.publish(msg)
msg.header.stamp = rospy.Time.now()
msg.value = value
msg.angle = angle
pub = rospy.Publisher('force_vect', vect_msg, queue_size=10)
pub.publish(msg)
rospy.loginfo('Force vector data sent')
def callback(light_sub, image_sub):
light_val = light_sub.illuminance
# Measurement of the Photometric Illuminance in Lux.
# rospy.loginfo('The illuminance value is %s\n', light_val)
msg = vect_msg()
#TODO define here a behavior according to the lux value
angle = 0
# Fill vector message
msg.angle = angle
msg.value = light_val
msg.header.stamp = rospy.Time.now()
pub = rospy.Publisher('light_vect', vect_msg, queue_size=10)
pub.publish(msg)
def wandering():
rospy.init_node('wandering', anonymous=True)
pub = rospy.Publisher('wander_vect', vect_msg, queue_size=10)
msg = vect_msg()
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
value = 1
angle = random.randint(1, 360)
# Fill vector message
msg.angle = angle
msg.value = value
msg.header.stamp = rospy.Time.now()
pub.publish(msg)
rospy.loginfo('Wander vector sent')
rate.sleep()
def callback(vel_sub, x_sub, pub):
print('aaaaaaaaa')
U = vect_msg()
Xest = Odometry()
cmd = Twist()
U = vel_sub
Xest = x_sub
# Variable assignation:
x_ref = U.value
x_est = Xest.twist.twist.linear.x
[yaw, pitch, roll] = get_rotation(Xest)
psi_ref = U.angle
psi_est = yaw*180/math.pi
# Error computation:
x_error = x_ref-x_est
psi_error = psi_ref-psi_est
# print(psi_est)
print(x_ref)
print(psi_ref)
# Control
x_cmd = x_error*0.25
psi_cmd = psi_error*0.3
# Normalization
psi_cmd = psi_cmd/180 #degrees
x_cmd = x_cmd/1
# Saturation
if psi_cmd > 0.5:
psi_cmd = 0.5
if psi_cmd < -0.5:
psi_cmd = -0.5
if x_cmd > 1:
x_cmd = 1
if x_cmd < -1:
x_cmd = -1
# Transform into velocity commands
cmd.linear.x = x_cmd
cmd.angular.z = psi_cmd
# Publishing
pub.publish(cmd)
rospy.loginfo("In the loop")
#!/usr/bin/env python
import rospy
import math
import random
from myrobot.msg import vect_msg
import message_filters
import vector_fcn as vectfcn
msg = vect_msg()
def arbiter():
rospy.init_node('arbiter',anonymous=True) # Create goal node called 'trajplan'
pub = rospy.Publisher('ref_vel',vect_msg,queue_size=10) #create goal publisher in the topic ref_vel, with vector_msg msgs
# and goal queue_size (size of outgoing message) of 10
rospy.loginfo("Status: Arbiter initialized")
# Subscribe to topics
rs_sub = message_filters.Subscriber('/rs_vect', vect_msg)
light_sub = message_filters.Subscriber('/light_vect', vect_msg)
force_sub = message_filters.Subscriber('/force_vect', vect_msg)
# Syncronize
ts = message_filters.TimeSynchronizer([rs_sub,light_sub,force_sub], queue_size=10)
# Register callback and publisher
ts.registerCallback(callback,pub)
rospy.spin()
def callback(rs_sub,light_sub,contact_sub,pub):
# Collecting vectors
vect_a = [rs_sub.angle, rs_sub.value]
vect_b = [force_sub.angle, force_sub.value]
vect_c = [light_sub.angle, light_sub.value]
# Sum vectors
def callback(color_raw, depth_raw,pub):
test = vect_msg()
msg = LaserScan()
bridge = CvBridge()
# realsense min and max distance
minDist = 0.3
maxDist = 4.5
# set show_depth to True to show the rgb and depth frames together
rs_plt = True # Display what is seen in color frames
show_depth = True
# Show images
if rs_plt:
cv2.namedWindow('RealSense color', cv2.WINDOW_AUTOSIZE)
cv2.namedWindow('RealSense depth', cv2.WINDOW_AUTOSIZE)
try:
try:
color_image = bridge.imgmsg_to_cv2(color_raw, "bgr8")
depth_image = bridge.imgmsg_to_cv2(depth_raw, "32FC1")
except CvBridgeError as e:
print(e)
depth_array = np.array(depth_image, dtype=np.float32)
norm_depth=cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
valX = np.arange(30,609,10)
valY = np.array([200])
dist = np.zeros(len(valX))
valid_matrix =np.zeros(len(valX))
color = np.asanyarray(color_image)
colorized_depth = np.asanyarray(depth_image)
for xCount in range(0,len(valX)):
distance = colorized_depth[valY[0],valX[xCount]]
if(distance < minDist) or math.isnan(distance)==True:
distance = np.inf
valid_matrix[xCount] = 0
else:
valid_matrix[xCount] = 1
dist[xCount] = distance
if rs_plt:
cv2.circle(color,(valX[xCount],valY[0]),5,(0,0,255),2)
cv2.circle(colorized_depth,(valX[xCount],valY[0]),5,(0,255,0),2)
# # Find index of valid values
print(dist)
RGB = np.dstack((norm_depth, np.zeros_like(norm_depth), np.zeros_like(norm_depth)))
grey_3_channel = cv2.cvtColor(norm_depth, cv2.COLOR_GRAY2BGR)
if rs_plt:
cv2.imshow('RealSense color', color)
if show_depth:
cv2.imshow('RealSense depth', grey_3_channel)
k = cv2.waitKey(1) & 0xFF
if k == ord('q'):
# break
pass
# Send data
msg.header.stamp=rospy.Time.now()
msg.header.frame_id='RealSense'
msg.angle_min = 0
msg.angle_max = 6.28319
msg.time_increment = 0; # instantaneous simulator scan
# msg.angle_increment=0.017
msg.scan_time = 0; # not sure whether this is correct
msg.range_min = 0.11
msg.range_max = 3.5
msg.ranges = dist
msg.intensities = np.zeros(len(dist))
# msg.header.stamp = rospy.Time.now()
# msg.angle = dist[1][1]
# msg.value = dist[1][0]
# rospy.loginfo('Realsense vector data sent')
pub.publish(msg)
finally:
pass
def callback(color_raw, depth_raw, pub):
print('abcdefghijk\n')
test = vect_msg()
msg = vect_msg()
bridge = CvBridge()
# realsense min and max distance
minDist = 0.3
maxDist = 4.5
# rs_plt = rospy.get_param("/rs_video")
# show_depth = rospy.get_param("/rs_depth")
# set show_depth to True to show the rgb and depth frames together
rs_plt = True # Display what is seen in color frames
show_depth = False
# Show images
if rs_plt:
cv2.namedWindow('RealSense', cv2.WINDOW_AUTOSIZE)
try:
# while not rospy.is_shutdown():
# # skip 5 first frames to give Auto-Exposure time to adjust
# for i in range(5):
# pipeline.wait_for_frames()
# Wait for a coherent pair of frames: depth and color
# frames = pipeline.wait_for_frames()
# color_frame = frames.get_color_frame()
# depth_frame = frames.get_depth_frame()
# Access color component of the frame
# color = np.asanyarray(color_frame.get_data())
try:
color_image = bridge.imgmsg_to_cv2(color_raw, "bgr8")
depth_image = bridge.imgmsg_to_cv2(depth_raw, "32FC1")
except CvBridgeError as e:
print(e)
depth_array = np.array(depth_image, dtype=np.float32)
cv2.normalize(depth_array, depth_array, 0, 1, cv2.NORM_MINMAX)
# Access depth component
# colorizer = rs.colorizer()
# colorizer_depth = np.asanyarray(colorizer.colorize(depth_frame).get_data()
# Depth and rgb image alignment
# align = rs.align(rs.stream.color)
# frames = align.process(frames)
# aligned_depth_frame = frames.get_depth_frame()
# colorized_depth = np.asanyarray(colorizer.colorize(aligned_depth_frame).get_data())
valX = np.array([30, 319, 609])
valY = np.array([30, 239, 449])
dist = np.zeros((3, 3))
valid_matrix = np.zeros((3, 3))
color = np.asanyarray(color_image)
colorized_depth = np.asanyarray(depth_image)
# print(colorized_depth)
# print(len(colorized_depth[1]))
# print((colorized_depth[120,1]))
# print((colorized_depth[120,319]))
for xCount in range(0, 3):
for yCount in range(0, 3):
distance = colorized_depth[valY[yCount], valX[xCount]]
if (distance < 0.3) or math.isnan(distance) == True:
distance = np.inf
valid_matrix[yCount, xCount] = 0
else:
valid_matrix[yCount, xCount] = 1
dist[yCount, xCount] = distance
if rs_plt:
cv2.circle(color, (valX[xCount], valY[yCount]), 5,
(0, 0, 255), 2)
cv2.circle(colorized_depth, (valX[xCount], valY[yCount]),
5, (0, 255, 0), 2)
# print("\033c")
# print(dist)
# # Find index of valid values
print(dist)
validIdxL = np.nonzero(valid_matrix[0:3, 0])
validIdxR = np.nonzero(valid_matrix[0:3, 2])
# if(((len(validIdxL[0]))>1 and len(validIdxR[0]))>1):
# angle = 180 - (np.mean(dist[validIdxL,0]) - np.mean(dist[validIdxR,2]))*180/np.pi
# value = 1/np.amin(dist)
# # #TODO remove print functions
# # print(validIdxL[0])
# # print(dist[validIdxL,0])
# # saturation
# if value > 1/minDist: # saturation to max speed
# value = 1.5
# if value<1/maxDist: # saturation to min speed
# value = 0
# else:
# angle = 0
# value = 0
if (((len(validIdxL[0])) > 1 and len(validIdxR[0])) > 1):
angle = 180 - (np.mean(dist[validIdxL, 0]) -
np.mean(dist[validIdxR, 2])) * 180 / np.pi
value = 1 / np.amin(dist)
# #TODO remove print functions
# print(validIdxL[0])
# print(dist[validIdxL,0])
# saturation
if value > 1 / minDist: # saturation to max speed
value = 1.5
if value < 1 / maxDist: # saturation to min speed
value = 0
else:
angle = 0
value = 0
print(angle, value)
vect = [angle, value]
#FIXME: make a simultaneous visualization of depth and color frames
if show_depth:
images = np.hstack((color, colorized_depth))
else:
images = color
if rs_plt:
cv2.imshow('RealSense', images)
k = cv2.waitKey(1) & 0xFF
if k == ord('q'):
# break
pass
# # Send data
msg.header.stamp = rospy.Time.now()
msg.angle = vect[0]
msg.value = vect[1]
rospy.loginfo('Realsense vector data sent')
pub.publish(msg)
# finally:
# # Stop streaming
# pipeline.stop()
finally:
pass
def callback(color_raw, depth_raw, pub):
test = vect_msg()
msg = vect_msg()
bridge = CvBridge()
# realsense min and max distance
minDist = 0.3
maxDist = 4.5
# set show_depth to True to show the rgb and depth frames together
rs_plt = True # Display what is seen in color frames
show_depth = True
# Show images
if rs_plt:
cv2.namedWindow('RealSense color', cv2.WINDOW_AUTOSIZE)
cv2.namedWindow('RealSense depth', cv2.WINDOW_AUTOSIZE)
try:
try:
color_image = bridge.imgmsg_to_cv2(color_raw, "bgr8")
depth_image = bridge.imgmsg_to_cv2(depth_raw, "32FC1")
except CvBridgeError as e:
print(e)
depth_array = np.array(depth_image, dtype=np.float32)
norm_depth = cv2.normalize(depth_array, depth_array, 0, 1,
cv2.NORM_MINMAX)
valX = np.array([30, 319, 609])
valY = np.array([30, 239, 449])
dist = np.zeros((3, 3))
valid_matrix = np.zeros((3, 3))
color = np.asanyarray(color_image)
colorized_depth = np.asanyarray(depth_image)
# print(colorized_depth)
# print(len(colorized_depth[1]))
# print((colorized_depth[120,1]))
# print((colorized_depth[120,319]))
for xCount in range(0, 3):
for yCount in range(0, 3):
distance = colorized_depth[valY[yCount], valX[xCount]]
if (distance < minDist) or math.isnan(distance) == True:
distance = np.inf
valid_matrix[yCount, xCount] = 0
else:
valid_matrix[yCount, xCount] = 1
dist[yCount, xCount] = distance
if rs_plt:
cv2.circle(color, (valX[xCount], valY[yCount]), 5,
(0, 0, 255), 2)
cv2.circle(colorized_depth, (valX[xCount], valY[yCount]),
5, (0, 255, 0), 2)
# print("\033c")
# print(dist)
# # Find index of valid values
print(dist)
validIdxL = np.nonzero(valid_matrix[0:3, 0])
validIdxR = np.nonzero(valid_matrix[0:3, 2])
# if(((len(validIdxL[0]))>1 and len(validIdxR[0]))>1):
# angle = 180 - (np.mean(dist[validIdxL,0]) - np.mean(dist[validIdxR,2]))*180/np.pi
# value = 1/np.amin(dist)
# # #TODO remove print functions
# # print(validIdxL[0])
# # print(dist[validIdxL,0])
# # saturation
# if value > 1/minDist: # saturation to max speed
# value = 1.5
# if value<1/maxDist: # saturation to min speed
# value = 0
# else:
# angle = 0
# value = 0
if (((len(validIdxL[0])) > 1 and len(validIdxR[0])) > 1):
angle = 180 - (np.mean(dist[validIdxL, 0]) -
np.mean(dist[validIdxR, 2])) * 180 / np.pi
value = 1 / np.amin(dist)
# #TODO remove print functions
# print(validIdxL[0])
# print(dist[validIdxL,0])
# saturation
if value > 1 / minDist: # saturation to max speed
value = 1.5
if value < 1 / maxDist: # saturation to min speed
value = 0
else:
angle = 0
value = 0
print(angle, value)
vect = [angle, value]
RGB = np.dstack(
(norm_depth, np.zeros_like(norm_depth), np.zeros_like(norm_depth)))
grey_3_channel = cv2.cvtColor(norm_depth, cv2.COLOR_GRAY2BGR)
if rs_plt:
cv2.imshow('RealSense color', color)
if show_depth:
cv2.imshow('RealSense depth', grey_3_channel)
k = cv2.waitKey(1) & 0xFF
if k == ord('q'):
# break
pass
# Send data
msg.header.stamp = rospy.Time.now()
msg.angle = dist[1][1]
msg.value = dist[1][0]
rospy.loginfo('Realsense vector data sent')
pub.publish(msg)
finally:
pass