def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
center_dict = {'blue': 'None', 'red': 'None', 'green': 'None'}
blue_center = Int64MultiArray()
red_center = Int64MultiArray()
green_center = Int64MultiArray()
image, center_dict, blue_center, red_center, green_center = self.image_centers(
cv_image, center_dict, blue_center, red_center, green_center)
try:
blue_direction = direction(blue_center, self.data_K, "blue")
except:
pass
try:
red_direction = direction(red_center, self.data_K, "red")
except:
pass
try:
green_direction = direction(green_center, self.data_K, "green")
except:
pass
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
if blue_direction.layout.dim[0].size != 0:
self.marker_blue.publish(blue_direction)
if red_direction.layout.dim[0].size != 0:
self.marker_red.publish(red_direction)
if green_direction.layout.dim[0].size != 0:
self.marker_green.publish(green_direction)
except CvBridgeError as e:
print(e)
def publisher():
sensor1 = serial.Serial('/dev/ttyACM0', 57600)
voc_pub = rospy.Publisher('VOC', Int64, queue_size=10)
coo_pub = rospy.Publisher('CO2', Int64, queue_size=10)
pm2_5_pub = rospy.Publisher('pm2_5', Int64, queue_size=10)
pm10_pub = rospy.Publisher('pm10', Int64, queue_size=10)
sensors_pub = rospy.Publisher('sensors', Int64MultiArray, queue_size=10)
rospy.init_node('air_sensors_publisher', anonymous=True)
rate = rospy.Rate(0.5)
print "CO2 and VOC publisher started"
sensors_data = Int64MultiArray()
while not rospy.is_shutdown():
try:
voc = sensor1.readline()
voc_pub.publish(int(voc))
co2 = sensor1.readline()
coo_pub.publish(int(co2))
pm2_5 = sensor1.readline()
pm2_5_pub.publish(int(pm2_5))
pm10 = sensor1.readline()
pm10_pub.publish(int(pm10))
sensors_data.data = [int(voc), int(co2), int(pm2_5), int(pm10)]
sensors_pub.publish(sensors_data)
except ValueError:
print "error type"
rate.sleep()
def execute(self, userdata):
time.sleep(5)
rospy.loginfo('Executing state NORMAL')
userdata.normal_counter_out = userdata.normal_counter_in + 1
## The robot moves randomly
for i in range(0, 3):
x = np.random.randint(0, 50, 1)
y = np.random.randint(0, 50, 1)
move_random = Int64MultiArray()
move_random.data = np.array([x, y])
## Publish random position on topic '/target_point'
target_pub.publish(move_random)
time.sleep(8)
## Subscribe to the topic '/vocal_comand' and '/pointed_comand'
rospy.Subscriber("/pointed_comand", Int64MultiArray,
callback_pointed_comand)
rospy.Subscriber("/vocal_comand", String, callback_vocal_comand)
if (vocal_data == 'go_to_home'):
## Check on user's vocal comands
return 'go_to_sleep'
rospy.loginfo('User decides to go to home')
if (vocal_data == 'play'):
rospy.loginfo('User decides to go to play')
return 'go_to_play'
time.sleep(4)
## Change state randomly: from 'NORMAL' to 'SLEEP' or 'PLAY'
return random.choice(['go_to_sleep', 'go_to_play'])
def execute(self, userdata):
time.sleep(5)
rospy.loginfo('Executing state PLAY')
userdata.play_counter_out = userdata.play_counter_in + 1
person_position = Int64MultiArray()
## Define random person position
x = np.random.randint(0, 50, 1)
y = np.random.randint(0, 50, 1)
person_position.data = np.array([x, y])
## Publish person position randomly on topic '/target_point'
target_pub.publish(person_position)
rospy.loginfo('The robot is reaching person position')
time.sleep(5)
rospy.loginfo('The robot is waiting for a pointing gesture')
time.sleep(10)
while True:
## Subscribe to the topic '/vocal_comand'
rospy.Subscriber("/vocal_comand", String, callback_vocal_comand)
if (vocal_data == 'go_to_home' or vocal_data == 'play'):
rospy.loginfo('ERROR: I am waiting for a pointing gesture')
time.sleep(5)
## Subscribe to the topic '/pointed_comand'
rospy.Subscriber("/pointed_comand", Int64MultiArray,
callback_pointed_comand)
time.sleep(5)
break
time.sleep(5)
## Change state randomly: from 'PLAY' to 'NORMAL'
return 'go_to_normal'
def callback(data):
global mode
global counter
global last
led_msg = Int64MultiArray()
y = data.axes[1]
x = data.axes[0]
x1 = data.axes[3]
rt = data.axes[2]
dpad = data.buttons[11:]
if 1 in dpad: mode = dpad.index(1)
now = time.time()
led_msg.data = [mode,1]
if now - last > 0.75:
counter +=1
else:
counter = 0
if counter > 3:
led_msg.data = [mode,0]
last = time.time()
led_pub.publish(led_msg)
#cmd = cartesian2polar_45(x,y)
cmd = two_joy(x1,y,rt)
joy_out = Twist()
joy_out.linear.x = cmd[0]
joy_out.angular.z = cmd[1]
pub.publish(joy_out)
def pub_values(self):
'''!
Функция публикации вершин
@return: null
'''
msg = Int64MultiArray()
msg.data = self.coordinates
self.publisher.publish(msg)
def determine_vision_mask(self, robot_pos):
# first determine the index position of robot_pos
robot_y_index, robot_x_index = self.index_pos(robot_pos)
#rospy.loginfo("Vision_Determiner: The robot x,y position is: %f, %f", robot_pos.pose.position.x, robot_pos.pose.position.y)
#rospy.loginfo("Vision_Determiner: The robot x, y index position is: %i, %i", robot_x_index, robot_y_index)
cols = self.occ_grid.info.width
rows = self.occ_grid.info.height
# create correct mask
mask = np.zeros((rows, cols), dtype=np.int64)
tested = np.zeros((rows, cols), dtype=np.int64)
# Vision Pattern needs to be odd number of cells
height = (self.vision_pattern.layout.dim[0].size - 1) / 2
width = (self.vision_pattern.layout.dim[1].size - 1) / 2
if (np.round(width) != width or np.round(height) != height):
rospy.loginfo('ERROR: Vision Patter is not odd height or length')
vp_width = self.vision_pattern.layout.dim[0].size
for i in range(-height, height + 1, 1):
for j in range(-width, width + 1, 1):
if self.vision_pattern.data[(i + height) * vp_width + (j + width)] == 1:
#[i + height][j + width] == 1:
new_i = i + robot_y_index
new_j = j + robot_x_index
# check if index is out of bounds
if new_i >= 0 and new_i < mask.shape[0] and new_j >= 0 and new_j < mask.shape[1]:
# check that not occupied by wall
if self.occ_grid.data[new_i * cols + new_j] == 0: # (new_i -1)
if tested[new_i][new_j] == 0: # has not been tested
# determine indeces along line
indeces = self.line_indeces(robot_x_index, robot_y_index, new_j, new_i)
vision_flag = True
for k in range(0,len(indeces)):
[v_x,v_y] = indeces[k]
tested[v_y][v_x] = 1
if self.occ_grid.data[v_y * cols + v_x] != 0:
vision_flag = False
elif vision_flag == True:
mask[v_y][v_x] = 1
if vision_flag == False:
mask[v_y][v_x] = 0
multi_array = Int64MultiArray()
multi_array.data = mask.flatten()
multi_array.layout.dim.append(MultiArrayDimension())
multi_array.layout.dim.append(MultiArrayDimension())
multi_array.layout.dim[0].size = cols
multi_array.layout.dim[0].label = "cols"
multi_array.layout.dim[1].size = rows
multi_array.layout.dim[1].label = "rows"
multi_array.layout.data_offset = 0
return multi_array
def numpy_to_multiarray(A):
strides = np.cumprod(A.shape[::-1])[::-1] # unused, but conforms to the ROS spec
layout = MultiArrayLayout(dim=[MultiArrayDimension(size=d, stride=s) for d, s in zip(A.shape, strides)])
if A.dtype == np.float32:
return Float32MultiArray(layout, A.reshape(-1).tolist())
elif A.dtype == np.float64:
return Float64MultiArray(layout, A.reshape(-1).tolist())
elif A.dtype == np.int32:
return Int32MultiArray(layout, A.reshape(-1).tolist())
elif A.dtype == np.int64:
return Int64MultiArray(layout, A.reshape(-1).tolist())
else:
raise TypeError
def publisher(value):
"""
Publisher
"""
pub = rospy.Publisher('tquad/arm', Int64MultiArray, queue_size=10)
rospy.init_node('tquad_arm', anonymous=True)
rate = rospy.Rate(1) # 1hz
arm = Int64MultiArray()
arm.data = []
while not rospy.is_shutdown():
arm.data = [value["q_1"], value["q_2"], value["q_3"], 1]
pub.publish(arm)
rate.sleep()
def publish_params():
pub_point = rospy.Publisher("point", Int64MultiArray,
queue_size=10) # point topic을 통해 point 값 전달
rospy.init_node('param_pub', anonymous=True)
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
x = random.randint(0, 300) # xmin
y = random.randint(0, 400) # ymin
w = random.randint(100, 150) # xmax = x + w
h = random.randint(100, 150) # ymax = y + h
point = (int((x + w) / 2), int((y + h) / 2))
array = [point[0], point[1]] # point 값을 배열로 저장
point = Int64MultiArray(data=array)
rospy.loginfo('publishing params')
rospy.loginfo(point)
pub_point.publish(point)
rate.sleep()
def talker():
##TODO: Define a publisher
pub = rospy.Publisher('chatter', Int64MultiArray, queue_size=1000)
rospy.init_node('talker', anonymous=True)
rate = rospy.Rate(10) # 10hz
count = 0
my_array = []
while not rospy.is_shutdown():
##TODO: For every loop, keep appending the array with the variable 'count' (If count = 4, an output array should be [1, 2, 3, 4])
my_array = np.append(my_array, count)
array_for_publish = Int64MultiArray(data=my_array)
##TODO: publish the message
pub.publish(array_for_publish)
rate.sleep()
count += 1
def send_to_face_detector(self, image):
try:
cv_image = self.cvb.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
print("\nSTART HERE!!\n")
send_data = cv_image.tolist()
self.client.send(send_data)
recv_data = self.client.recv()
recv_data = msgpack.unpackb(recv_data)
print("recv_data [p1, p2] : ", recv_data)
face_points = Int64MultiArray(data=recv_data[0]+recv_data[1])
self.pub_face_point.publish(face_points)
self.rate.sleep()
def Simulator():
global user_comand
while not rospy.is_shutdown():
## 2D coordinates generation (pointed gestures)
x = np.random.randint(0, 50, 1)
y = np.random.randint(0, 50, 1)
array_point = np.array([x, y])
## User comands (speak interaction and pointed gestures)
user_comand = random.choice(['go_to_home', 'play', array_point])
print(user_comand)
## Declare Publisher
vocal_comand_pub = rospy.Publisher('/vocal_comand',
String,
queue_size=10)
pointed_comand_pub = rospy.Publisher('/pointed_comand',
Int64MultiArray,
queue_size=10)
## Inizialize the node
rospy.init_node('sim_perception', anonymous=True)
## Inizialize the variable which contains the 2D point
pointed_comand = Int64MultiArray()
## Check if "user_comand" is a string or an array
if type(user_comand) == str:
rospy.loginfo(user_comand)
## Publish 'user_comand' on the topic '/vocal_comand'
vocal_comand_pub.publish(user_comand)
time.sleep(25)
else:
pointed_comand.data = user_comand
rospy.loginfo(pointed_comand)
## Publish 'user_comand' on the topic '/pointed_comand'
pointed_comand_pub.publish(pointed_comand)
time.sleep(25)
def callback(self, data):
global tick
tick = tick + 1
if tick % 43 == 0:
tick = 0
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
im = PImage.fromarray(cv2.cvtColor(cv_image, cv2.COLOR_BGR2RGB))
im = im.convert('RGB')
results = face_recognition.face_locations(np.array(im), 2)
#print(results)
if len(results) > 0:
x1 = results[0][0]
y1 = results[0][1]
x2 = results[0][2]
y2 = results[0][3]
msg = Int64MultiArray()
msg.data = [x1, y1, x2, y2]
pub.publish(msg)
cv2.rectangle(cv_image, (y1, x1), (y2, x2), (0, 255, 0), 5)
cv2.imshow("Locate face", cv_image)
cv2.waitKey(3)
def pub_center(self, img, cx, cy, corners2D):
if corners2D is not None:
wp = PoseStamped()
wp.header.stamp = rospy.Time.now()
wp.header.frame_id = ""
wp.pose.position.x = cx
wp.pose.position.y = cy
self.wp_pub.publish(wp)
size = Int64MultiArray()
px = corners2D[3][0] - corners2D[1][0]
py = corners2D[3][1] - corners2D[1][1]
size.data.append(int(px))
size.data.append(int(py))
self.size_pub.publish(size)
cv2.circle(img, (px, py), 3, (0, 255, 0), 3)
imgmsg = self.bridge.cv2_to_imgmsg(img, "rgb8")
self.img_with_det.publish(imgmsg)
def callback(data): global mode global counter global last led_msg = Int64MultiArray() joy_out = Joystick() y = data.axes[1] x = -data.axes[0] x1 =-data.axes[3] rt = data.axes[2] cmd = two_joy(x1,y,rt) dpad = data.buttons[11:] if 1 in dpad: mode = dpad.index(1) now = time.time() led_msg.data = [mode,1] if now - last > 0.75: counter +=1 else: counter = 0 if counter > 3: led_msg.data = [mode,0] last = time.time() led_pub.publish(led_msg) #cmd = cartesian2polar_45(x,y) cmd = two_joy(x1,y,rt) joy_out = Joystick() joy_out.vel = cmd[0] joy_out.steering = cmd[1] joy_out.mode = mode joy_out.connected = True pub.publish(joy_out)
def execute(self, userdata):
time.sleep(5)
rospy.loginfo('Executing state SLEEP')
userdata.sleep_counter_out = userdata.sleep_counter_in + 1
## Comand that let the robot go home
home_comand = Int64MultiArray()
## Define home position
home_comand.data = np.array([0, 0])
## Publish home position on topic '/target_point'
target_pub.publish(home_comand)
rospy.loginfo('The robot is reaching home position')
time.sleep(15)
## Subscribe to the topic '/vocal_comand'
rospy.Subscriber("/vocal_comand", String, callback_vocal_comand)
if vocal_data == 'go_to_home':
time.sleep(5)
rospy.loginfo('The robot is already at home')
else:
time.sleep(4)
## Change state: from 'SLEEP' to 'NORMAL'
return 'wake_up'
def test_talker():
rospy.init_node('test_talker')
#obstacles_publisher = rospy.Publisher('/path/obstacles', Int64MultiArray, queue_size=10)
number_obstacles_publisher = rospy.Publisher('/path/number_obstacles',
Int64MultiArray,
queue_size=10)
lim_obstacles_publisher = rospy.Publisher('/path/lim_obstacles',
Int64MultiArray,
queue_size=10)
path_planing_param_publisher = rospy.Publisher('/path/planing_param',
Int64MultiArray,
queue_size=10)
start_point_publisher = rospy.Publisher('/start_point/position',
Point,
queue_size=10)
goal_point_publisher = rospy.Publisher('/goal_point/position',
Point,
queue_size=10)
#max_iter_publisher = rospy.Publisher('/path/max_iter', Int64MultiArray, queue_size=10)
#step_size_publisher = rospy.Publisher('/path/step_size', Int64MultiArray, queue_size=10)
#goal_reach_thresh_publisher = rospy.Publisher('/path/goal_reach_thresh', Int64MultiArray, queue_size=10)
#drone_radius_publisher = rospy.Publisher('/path/drone_radius', Int64MultiArray, queue_size=10)
#obstacles = Int64MultiArray()
number_obstacles = Int64MultiArray()
lim_obstacles = Int64MultiArray()
path_planing_param = Int64MultiArray()
start_point = Point()
goal_point = Point()
number_obstacles = 0
start_point.x = -5001.787385427393
start_point.y = 9526.705177228898
goal_point.x = -2833
goal_point.y = 4969
_xlim = -6000
xlim = -1000
_ylim = 4000
ylim = 10000
max_iter = 50000
step_size = 400
goal_reach_thresh = 1000
drone_radius = 2
lim_obstacles.data = [_xlim, xlim, _ylim, ylim]
path_planing_param.data = [
max_iter, step_size, goal_reach_thresh, drone_radius
]
#max_iter = Int64MultiArray()
#step_size = Int64MultiArray()
#goal_reach_thresh = Int64MultiArray
#drone_radius = Int64MultiArray()
#rate = rospy.Rate(10) # 10hz
#while not rospy.is_shutdown():
#connections = goal_point.publish.get_num_connections()
#rospy.loginfo('Connections: %d', connections)
#if connections > 0:
lim_obstacles_publisher.publish(lim_obstacles)
path_planing_param_publisher.publish(path_planing_param)
goal_point_publisher.publish(goal_point)
start_point_publisher.publish(start_point)
class Vision_Determiner:
# Subscribers
r0_pos_topic = "/tb3_0/amcl_pose"
r1_pos_topic = "/tb3_1/amcl_pose"
occ_grid_topic = "/modified_occupancy_grid"
vision_pattern_topic = "/vision_pattern"
# Containers for Subscribed content
r0_pos = PoseWithCovarianceStamped()
r1_pos = PoseWithCovarianceStamped()
occ_grid = OccupancyGrid()
vision_pattern = Int64MultiArray()
# Published content
r0_current_vision = Int64MultiArray() #np.zeros((1,1))
r1_current_vision = Int64MultiArray() #np.zeros((1,1))
# Publisher
r0_current_vision_topic = "/tb3_0/current_vision"
r1_current_vision_topic = "/tb3_1/current_vision"
pub_r0_current_vision = rospy.Publisher(r0_current_vision_topic, Int64MultiArray, queue_size = 100)
pub_r1_current_vision = rospy.Publisher(r1_current_vision_topic, Int64MultiArray, queue_size = 100)
def print_map(self):
# Only for testing issues: prints new_map
# reverse the list for a second view on the map:
# copy list without reference to old one
reverse_map = []
for i in range(len(self.r1_current_vision.data)):
reverse_map.append(self.r1_current_vision.data[i])
reverse_map.reverse()
rospy.loginfo(rospy.get_caller_id(
) + "\tThe current transformed occupancy grid is the following one (free = ':' [0], occupied = 'X' [100 or -1]):")
string = ""
string_rev = ""
# cannot use "i in new_map" because I need the real index for the line break
for index in range(0, len(self.r1_current_vision.data)):
if index % 20 == 0:
string += "\n"
string_rev += "\n"
if self.r1_current_vision.data[index] == 0:
string += ": "
else:
string += "X "
if reverse_map[index] == 0:
string_rev += ": "
else:
string_rev += "X "
print("\nPerspective with (-5/-5) in the upper left corner (actual order of the map):" + string)
print("\nPerspective with (5/5) in the upper left corner (standard perspective in simulation):" + string_rev + "\n")
def r0_pos_call_back(self, msg):
self.r0_pos = msg
def r1_pos_call_back(self, msg):
self.r1_pos = msg
def occ_grid_call_back(self, msg):
self.occ_grid = msg
def vision_pattern_call_back(self, msg):
self.vision_pattern = msg
def subscribe_to_topics(self):
rospy.Subscriber(self.r0_pos_topic, PoseWithCovarianceStamped, self.r0_pos_call_back)
rospy.Subscriber(self.r1_pos_topic, PoseWithCovarianceStamped, self.r1_pos_call_back)
rospy.Subscriber(self.occ_grid_topic, OccupancyGrid, self.occ_grid_call_back)
rospy.Subscriber(self.vision_pattern_topic, Int64MultiArray, self.vision_pattern_call_back)
def publish_to_topics(self):
self.pub_r0_current_vision.publish(self.r0_current_vision)
self.pub_r1_current_vision.publish(self.r1_current_vision)
def index_pos(self, robot_pos):
resolution = self.occ_grid.info.resolution
origin = self.occ_grid.info.origin
width_distance = robot_pos.pose.position.y - origin.position.y
width_index = int(width_distance / resolution)
height_distance = robot_pos.pose.position.x - origin.position.x
height_index = int(height_distance / resolution)
return height_index, width_index
def line_indeces(self, r_x, r_y, i_x, i_y):
indeces = []
x_diff = i_x - r_x
y_diff = i_y - r_y
if x_diff == 0 and y_diff == 0:
indeces.append([r_x, r_y])
elif x_diff == 0:
for i in range(r_y, i_y + np.sign(y_diff), np.sign(y_diff)):
indeces.append([r_x, i])
elif y_diff == 0:
for i in range(r_x, i_x + np.sign(x_diff), np.sign(x_diff)):
indeces.append([i,r_y])
else:
m = float(y_diff) / x_diff
c = r_y - m * r_x
if np.abs(m) > 1:
step_size = np.abs(1 / m)
num_steps = np.abs(y_diff)
for i in range(1, num_steps + 1):
x = step_size * i * np.sign(x_diff) + r_x
y = m * x + c
indeces.append([int(x), int(y)])
else:
step_size = np.abs(m)
num_steps = np.abs(x_diff)
for i in range(1, num_steps + 1):
y = step_size * i * np.sign(y_diff) + r_y
x = (y - c) / m
indeces.append([int(x), int(y)])
return indeces
def determine_vision_mask(self, robot_pos):
# first determine the index position of robot_pos
robot_y_index, robot_x_index = self.index_pos(robot_pos)
#rospy.loginfo("Vision_Determiner: The robot x,y position is: %f, %f", robot_pos.pose.position.x, robot_pos.pose.position.y)
#rospy.loginfo("Vision_Determiner: The robot x, y index position is: %i, %i", robot_x_index, robot_y_index)
cols = self.occ_grid.info.width
rows = self.occ_grid.info.height
# create correct mask
mask = np.zeros((rows, cols), dtype=np.int64)
tested = np.zeros((rows, cols), dtype=np.int64)
# Vision Pattern needs to be odd number of cells
height = (self.vision_pattern.layout.dim[0].size - 1) / 2
width = (self.vision_pattern.layout.dim[1].size - 1) / 2
if (np.round(width) != width or np.round(height) != height):
rospy.loginfo('ERROR: Vision Patter is not odd height or length')
vp_width = self.vision_pattern.layout.dim[0].size
for i in range(-height, height + 1, 1):
for j in range(-width, width + 1, 1):
if self.vision_pattern.data[(i + height) * vp_width + (j + width)] == 1:
#[i + height][j + width] == 1:
new_i = i + robot_y_index
new_j = j + robot_x_index
# check if index is out of bounds
if new_i >= 0 and new_i < mask.shape[0] and new_j >= 0 and new_j < mask.shape[1]:
# check that not occupied by wall
if self.occ_grid.data[new_i * cols + new_j] == 0: # (new_i -1)
if tested[new_i][new_j] == 0: # has not been tested
# determine indeces along line
indeces = self.line_indeces(robot_x_index, robot_y_index, new_j, new_i)
vision_flag = True
for k in range(0,len(indeces)):
[v_x,v_y] = indeces[k]
tested[v_y][v_x] = 1
if self.occ_grid.data[v_y * cols + v_x] != 0:
vision_flag = False
elif vision_flag == True:
mask[v_y][v_x] = 1
if vision_flag == False:
mask[v_y][v_x] = 0
multi_array = Int64MultiArray()
multi_array.data = mask.flatten()
multi_array.layout.dim.append(MultiArrayDimension())
multi_array.layout.dim.append(MultiArrayDimension())
multi_array.layout.dim[0].size = cols
multi_array.layout.dim[0].label = "cols"
multi_array.layout.dim[1].size = rows
multi_array.layout.dim[1].label = "rows"
multi_array.layout.data_offset = 0
return multi_array
def spin(self):
r = rospy.Rate(1)
while not rospy.is_shutdown():
#rospy.spin()
self.r0_current_vision = self.determine_vision_mask(self.r0_pos.pose)
self.r1_current_vision = self.determine_vision_mask(self.r1_pos.pose)
# rospy.loginfo("Vision_Determiner: Publishing to topics")
self.publish_to_topics()
#self.print_map()
r.sleep()
class Vision_Discretizer:
# Subscribers
sensor_topic = 'tb3_0/scan'
occ_grid_topic = 'modified_occupancy_grid'
# Containers for Subscribed content
occ_grid = OccupancyGrid()
sensor = LaserScan()
# Published content
vision_pattern = Int64MultiArray()
# Publisher
vision_pattern_topic = "vision_pattern"
pub_vision_pattern = rospy.Publisher(vision_pattern_topic,
Int64MultiArray,
queue_size=100)
def occ_grid_call_back(self, msg):
self.occ_grid = msg
def sensor_call_back(self, msg):
self.sensor = msg
def subscribe_to_topics(self):
rospy.Subscriber(self.sensor_topic, LaserScan, self.sensor_call_back)
rospy.Subscriber(self.occ_grid_topic, OccupancyGrid,
self.occ_grid_call_back)
def publish_to_topics(self):
self.pub_vision_pattern.publish(self.vision_pattern)
def determine_vision_pattern(self):
resolution = self.occ_grid.info.resolution
sensor_range = self.sensor.range_max
dimension = 2 * int(
np.round(math.ceil((sensor_range) / resolution - 0.5))) + 1
self.vision_pattern.data = []
self.vision_pattern.layout.dim.append(MultiArrayDimension())
self.vision_pattern.layout.dim.append(MultiArrayDimension())
self.vision_pattern.layout.dim[0].size = dimension
self.vision_pattern.layout.dim[1].size = dimension
self.vision_pattern.layout.dim[0].label = "rows"
self.vision_pattern.layout.dim[1].label = "cols"
self.vision_pattern.layout.data_offset = 0
centre_index = (dimension + 1) / 2 - 1 # -1 since index starts at 0
for i in range(0, dimension):
for j in range(0, dimension):
distance = resolution * np.sqrt((i - centre_index)**2 +
(j - centre_index)**2)
if distance <= sensor_range:
self.vision_pattern.data.append(1)
else:
self.vision_pattern.data.append(0)
def spin(self):
rospy.loginfo("Vision_Discretizer: Now discretizing vision")
self.determine_vision_pattern()
print(self.vision_pattern)
rospy.loginfo(
"Vision_Discretizer: Publishing vision every second from now on")
r = rospy.Rate(1)
while not rospy.is_shutdown():
self.publish_to_topics()
r.sleep()
def extract_data(all_data):
'''Read an ensemble of ADCP data.
Publish with the adcp_pub on topic /adcp/pub
Args:
all_data (bytestring): an bytestring that contains an ensemble
(one package) of adcp data
Return:
float list: essential adcp measurements
[heading, roll, pitch, transducer_depth, bt_ranges, bt_velocities,
surface_vel]
bt_ranges, bt_velocities, surface vel is floats long'''
num_types = all_data[5]
# OFFSETS
# 1. Fix Leader
# 2. Variable Leader
# 3. Velocity Profile
# 6. Bottom Track
# 9. Vertical Beam Range
# 10-13. GPS
offsets = []
for i in range(int(num_types.encode('hex'), 16)):
offset = all_data[6 + 2 * i:8 + 2 * i]
offset_int = int(''.join(reversed(offset)).encode('hex'), 16)
offsets.append(offset_int)
# print('Offsets: ', offsets)
# print('Data IDs: ', [all_data[x:x+2] for x in offsets])
# python3 way of extracting hex value int.from_bytes(data, byteorder='little')
# python2 way of extracting int(''.join(reversed(data)).encode('hex'), 16)
# reference https://stackoverflow.com/questions/32014475/unable-convert-to-int-from-bytes
# FIXED LEADER
fixed_offset = offsets[0]
num_beams = all_data[fixed_offset + 8]
num_cells = all_data[fixed_offset + 9]
pings_per_ensemble = int(
''.join(reversed(all_data[fixed_offset + 10:fixed_offset +
12])).encode('hex'), 16)
depth_cell_length = int(
''.join(reversed(all_data[fixed_offset + 12:fixed_offset +
14])).encode('hex'), 16)
coord_transform = all_data[fixed_offset + 25]
heading_alignment = int(
''.join(reversed(all_data[fixed_offset + 26:fixed_offset +
28])).encode('hex'), 16)
# print('Heading alignment: ', heading_alignment)
# VARIABLE LEADER
variable_offset = offsets[1]
transducer_depth = int(''.join(
reversed(all_data[variable_offset + 14:variable_offset +
16])).encode('hex'), 16) #1 dm
heading = s16(
int(
''.join(
reversed(all_data[variable_offset + 18:variable_offset +
20])).encode('hex'), 16)) # degree
pitch = s16(
int(
''.join(
reversed(all_data[variable_offset + 22:variable_offset +
24])).encode('hex'), 16)) # degree
roll = s16(
int(
''.join(
reversed(all_data[variable_offset + 20:variable_offset +
22])).encode('hex'), 16)) # degree
salinity = int(''.join(
reversed(all_data[variable_offset + 24:variable_offset +
26])).encode('hex'), 16) # 0 to 40 ppm
temperature = int(''.join(
reversed(all_data[variable_offset + 26:variable_offset +
28])).encode('hex'), 16) # degree
# print('HEADING', heading)
# VELOCITY PROFILE
velocity_profile_offset = offsets[2]
relative_velocities = []
# TODO: figure out coordinate transform once more
for i in range(int(num_cells.encode('hex'), 16)):
start_offset = velocity_profile_offset + 2 + 8 * i
# Average over beams
vel = []
for j in range(int(num_beams.encode('hex'), 16)):
curVel = s16(
int(
''.join(
reversed(all_data[start_offset + 2 * j:start_offset +
2 + 2 * j])).encode('hex'), 16))
# curVel = int.from_bytes(all_data[start_offset + 2*j: start_offset + 2 + 2*j], byteorder='little', signed=True)
vel.append(curVel)
relative_velocities.append(vel)
# BOTTOM TRACK (abbr. bt) (see page 154)
# Coordinate system for velocity:
# 1. Earth Axis (default): East, North, Up (right hand orthogonal)
# need to set heading alignment (EA), heading bias (EB) correctly
# make sure heading sensors are active (EZ)
# 2. Radial Beam Coordinates: "raw beam measurements" (not orthogonal)
# 3. Instrument coordinates: X, Y, UP, X is directon of beam 2, Y is beam 3. Compass
# measures the offset of Y from magnetic north
# 4. Ship coordinates: starboard, forward, mast (pitch, roll, yaw)
bt_offset = offsets[5]
bt_pings_per_ensemble = int(
''.join(reversed(all_data[bt_offset + 2:bt_offset + 4])).encode('hex'),
16)
bt_ranges = [
] # ranges measurement for each beam (bt = 0 is bad data) # cm
bt_velocities = [
] # there are one more velocity data.. though not sure what it's for?
beam_percent_good = []
max_tracking_depth = int(
''.join(reversed(all_data[bt_offset + 70:bt_offset +
72])).encode('hex'), 16)
for i in range(4):
bt_ranges.append(
int(
''.join(
reversed(all_data[bt_offset + 16 + i * 2:bt_offset + 18 +
i * 2])).encode('hex'), 16))
bt_velocities.append(
s16(
int(
''.join(
reversed(all_data[bt_offset + 24 + i * 2:bt_offset +
26 + i * 2])).encode('hex'), 16)))
beam_percent_good.append(all_data[bt_offset + 40 + i])
# VERTICAL BEAM RANGE
vb_offset = offsets[8]
vb_range = int(''.join(reversed(all_data[vb_offset + 4:vb_offset +
8])).encode('hex'),
16) # in millimeter
# GPS Data
GPS_offsets = offsets[9:13]
msg_types = []
msg_sizes = []
delta_times_bytes = []
delta_times_double = [] # difference between GPS message and ensemble
GPS_msg = []
for g_offset in GPS_offsets:
msg_size = int(
''.join(reversed(all_data[g_offset + 4:g_offset +
6])).encode('hex'), 16)
msg_types.append(
int(
''.join(reversed(all_data[g_offset + 2:g_offset +
4])).encode('hex'), 16))
msg_sizes.append(msg_size)
delta_times_bytes.append(all_data[g_offset + 6:g_offset + 14])
GPS_msg.append(all_data[g_offset + 15:g_offset + 15 + msg_size])
delta_times_double = [struct.unpack('d', b)[0]
for b in delta_times_bytes] # convert to double
surface_vel = relative_velocities[0]
essential = [heading, roll, pitch, transducer_depth]
essential = essential + bt_ranges
essential = essential + bt_velocities
essential = essential + surface_vel
message = Int64MultiArray()
message.data = essential
adcp_pub.publish(message)
return essential
def field_cv_pub():
# initialize ros node
rospy.init_node("field_cv")
# Defining ARUCO libraries
arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_5X5_100)
arucoParams = cv2.aruco.DetectorParameters_create()
# Starting system timer
start = timeit.default_timer()
# Setting up camera capture parameters
capture = cv2.VideoCapture(2)
capture.set(cv2.CAP_PROP_FRAME_WIDTH, 1920)
capture.set(cv2.CAP_PROP_FRAME_HEIGHT, 1080)
check, img = capture.read()
fourcc = cv2.VideoWriter_fourcc(*'MP4V')
fvid = cv2.VideoWriter('NavigationTest.mp4', fourcc, 10.0, (1420, 1080))
# img = cv2.imread('Field2.jpg') # Use if no camera
# Grabs defishing variables and resizes the image
fpath = os.path.join(os.path.dirname(__file__), "camParams.txt")
K, D, w, h = get_params_from_file(fpath)
img = cv2.resize(img, (w, h))
newcammtx, roi = cv2.getOptimalNewCameraMatrix(K, D, (w, h), 0, (w, h))
img_new = cv2.undistort(img, K, D, None, newcammtx)
x, y, w, h = [265, 0, 1420, h]
img_new = img_new[y:y + h, x:x + w]
# get shape of image
shape = img_new.shape
# calculate inches per pixel
ippx = 144.0 / shape[1]
ippy = 94.0 / shape[0]
# convert to grayscale
gray = cv2.cvtColor(img_new, cv2.COLOR_BGR2GRAY)
# apply gaussian blur
blur = cv2.GaussianBlur(gray, (3, 3), 0)
# cv2.imwrite('blur.jpg', blur)
# apply canny edge detection and save
edges = cv2.Canny(blur, 80, 240, apertureSize=3)
# cv2.imwrite('edges.jpg', edges)
# set parameters for hough lines and generate world lines
minLL = 200
maxLG = 5
lines = cv2.HoughLinesP(edges, 1, np.pi / 180, 20, minLL, maxLG)
wrld_lines = np.empty([len(lines), 4], dtype=float)
for i in range(len(lines) - 1):
for x1, y1, x2, y2 in lines[i]:
cv2.line(img_new, (x1, y1), (x2, y2), (0, 255, 0), 2)
wrld_lines[i] = np.array([(((x1 * 2) - shape[1]) * ippx) / 2,
((shape[0] - y1 * 2) * ippy) / 2,
((x2 * 2 - shape[1]) * ippx) / 2,
((shape[0] - y2 * 2) * ippy) / 2],
dtype=float)
# Store world lines to text file
f = open('world_lines.txt', 'w')
for i in range(len(wrld_lines) - 1):
for j in range(4):
f.write(str(wrld_lines[i][j]) + ' ')
f.write('\n')
# Detect ARUCO markers
(corners, ids, rejected) = cv2.aruco.detectMarkers(gray,
arucoDict,
parameters=arucoParams)
#cv2.imshow("disp gray", gray)
print(ids)
id_pub = rospy.Publisher("ids", Int64MultiArray, queue_size=1)
ids_msg = Int64MultiArray()
ids_2 = []
for item in ids:
ids_2.append(item[0])
ids_msg.data = ids_2
id_pub.publish(ids_msg)
publishers = []
goal_publishers = []
avs = []
publisher_ids = []
# Plot avs if they are found
if ids is not None:
av1 = AV(img_new, corners[0][0], ippx, ippy, shape, ids[0])
print(av1.get_pos()) # Prints the coords and heading angle for av1
publisher_ids.append(ids[0])
av1.plot() # Plots the coords and heading angle for av1
pub = rospy.Publisher("pose{}".format(ids[0][0]),
Float64MultiArray,
queue_size=1)
goal_pub = rospy.Publisher("goals{}".format(ids[0][0]),
Float64MultiArray,
queue_size=1)
publishers.append(pub)
goal_publishers.append(goal_pub)
avs.append(av1)
if len(corners) > 1:
av2 = AV(img_new, corners[1][0], ippx, ippy, shape, ids[1])
publisher_ids.append(ids[1])
pub = rospy.Publisher("pose{}".format(ids[1][0]),
Float64MultiArray,
queue_size=1)
goal_pub = rospy.Publisher("goals{}".format(ids[1][0]),
Float64MultiArray,
queue_size=1)
goal_publishers.append(goal_pub)
print(av2.get_pos()) # Prints the coords and heading angle for av2
av2.plot() # Plots the coords and heading angle for av2
publishers.append(pub)
avs.append(av2)
# Plots figure with dimensions of the field
plt.figure()
plt.title('Field plot')
ext = [-72.0, 72.0, -47.00, 47.0]
plt.imshow(img_new, zorder=0, extent=ext)
plt.show()
goals = []
goals.append(np.array([-4 * 12, 1 * 12]))
goals.append(np.array([4 * 12, -1 * 12]))
paths = []
if len(publishers) > 0:
for i, pub in enumerate(publishers):
pose = avs[i].get_pos()
msg = Float64MultiArray()
msg.data = [pose[0], pose[1], pose[2]]
pub.publish(msg)
if ids[i] == 0:
goals_car = traj_plan(pose[0:2], goals[0])
else:
goals_car = traj_plan(pose[0:2], goals[1])
msg = Float64MultiArray()
msg.data = goals_car
paths.append(msg)
for i, path in enumerate(paths):
goal_publishers[i].publish(path)
#agent_poses = []
#agent_pose = avs[0].get_pos()
#agent_poses.append(agent_pose[0:2])
#goals = []
#goals.append(traj_plan(agent_pose, np.array([4*12, -1*12])))
# Stops initiation timer
stop = timeit.default_timer()
print('Time: ', stop - start)
if ids is not None:
while True:
start = timeit.default_timer()
ret, frame = capture.read()
frame_dst = cv2.undistort(frame, K, D, None, newcammtx)
x, y, w, h = [265, 0, 1420, 1080]
frame_dst = frame_dst[y:y + h, x:x + w]
grey = cv2.cvtColor(frame_dst, cv2.COLOR_BGR2GRAY)
(corners, ids,
rejected) = cv2.aruco.detectMarkers(grey,
arucoDict,
parameters=arucoParams)
if ids is not None:
frame_test = cv2.aruco.drawDetectedMarkers(
frame_dst, corners, ids)
car1 = AV(frame_dst, corners[0][0], ippx, ippy, shape, ids[0])
pose = car1.get_pos()
msg = Float64MultiArray()
msg.data = [pose[0], pose[1], pose[2]]
if ids[0] == publisher_ids[0]:
publishers[0].publish(msg)
elif ids[0] == publisher_ids[1]:
publishers[1].publish(msg)
print(car1.get_pos())
car1.plot()
if len(corners) > 1:
car2 = AV(frame_dst, corners[1][0], ippx, ippy, shape,
ids[1])
print(car2.get_pos())
pose = car2.get_pos()
msg = Float64MultiArray()
msg.data = [pose[0], pose[1], pose[2]]
if ids[1] == publisher_ids[0]:
publishers[0].publish(msg)
elif ids[1] == publisher_ids[1]:
publishers[1].publish(msg)
car2.plot()
del ids, corners
cv2.imshow('pls wrk', frame_dst)
if ret == True:
fvid.write(frame_dst)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
time.sleep(.04)
stop = timeit.default_timer()
print('Time: ', stop - start)
fvid.release()
capture.release()
cv2.destroyAllWindows()
def CAN_recv():
# Set up node
rospy.init_node('CAN_broadcaster', anonymous=True)
# Set CAN interface
can_interface = 'can0'
bus = can.interface.Bus(can_interface, bustype='socketcan')
# Publisher
pub = rospy.Publisher('CAN_bus', Int64MultiArray, queue_size=10)
pub_bc = rospy.Publisher('CAN_bus/battery_current',
Int64MultiArray,
queue_size=10)
pub_cc = rospy.Publisher('CAN_bus/charge_current',
Int64MultiArray,
queue_size=10)
pub_bv = rospy.Publisher('CAN_bus/battery_voltage',
Int64MultiArray,
queue_size=10)
# Create new msgs
can_msg_bat_curr = Int64MultiArray()
can_msg_bat_curr.data = [0, 0]
can_msg_chrg_curr = Int64MultiArray()
can_msg_chrg_curr.data = [0, 0]
can_msg_batt_volt = Int64MultiArray()
can_msg_batt_volt.data = [0, 0]
while not rospy.is_shutdown():
message = bus.recv() # Wait for new CAN message
# Publish msgs
if message is not None:
if message.arbitration_id is 16: # Battery current
can_msg_bat_curr.data[0] = message.arbitration_id
try:
res = struct.unpack('<i', message.data)
except:
pass
else:
can_msg_bat_curr.data[1] = res[0]
pub.publish(can_msg_bat_curr)
pub_bc.publish(can_msg_bat_curr)
if message.arbitration_id is 17: # Charger current
can_msg_chrg_curr.data[0] = message.arbitration_id
try:
res = struct.unpack('<i', message.data)
except:
pass
else:
can_msg_chrg_curr.data[1] = res[0]
pub_cc.publish(can_msg_chrg_curr)
if message.arbitration_id is 18: # Battery voltage
can_msg_batt_volt.data[0] = message.arbitration_id
try:
res = struct.unpack('<i', message.data)
except:
pass
else:
can_msg_batt_volt.data[1] = res[0]
pub_bv.publish(can_msg_batt_volt)
bus.shutdown()
def main():
windup=20
sampletime=0.01
mpfh=MobilePlatFormHoming()
mpfh.Init_mobile_platform()
# mpfh.Init_Pid_Control(P,I,D,windup,sampletime)
mpfh.Init_Ros_Node()
P=mpfh.MobileControl.CanAnalysis.yamlDic['Pid_parameter']['P']
I=mpfh.MobileControl.CanAnalysis.yamlDic['Pid_parameter']['I']
D=mpfh.MobileControl.CanAnalysis.yamlDic['Pid_parameter']['D']
count=1
flag=0
recevenum=0
flag_1=1
count_num_fl=0
count_num_fr=0
count_num_rl=0
count_num_rr=0
flag_fl = 'fl'
flag_fr= 'fr'
flag_rl= 'rl'
flag_rr= 'rr'
END=100
ratet=1
homing_ok_dic={}
######fl
output_fl=[]
output_fr=[]
output_rl=[]
output_rr=[]
feedback_fl=0
feedback_list_fl = []
time_list_fl = []
setpoint_list_fl = []
status_ok_fl=0
#####fr
feedback_fr=0
feedback_list_fr = []
time_list_fr = []
setpoint_list_fr = []
status_ok_fr=0
#####rl
feedback_rl=0
feedback_list_rl = []
time_list_rl = []
setpoint_list_rl = []
status_ok_rl=0
#######rr
feedback_rr=0
feedback_list_rr = []
time_list_rr = []
setpoint_list_rr = []
status_ok_rr=0
#######
rate = rospy.Rate(ratet)
while not rospy.is_shutdown():
# print "haha"
recevenum=mpfh.MobileControl.CanAnalysis.Can_GetReceiveNum(0)
# print recevenum
# mpfh.New_Read_Encoder_data_From_ABS_Encoder(recevenum)
if recevenum!=None:
if mpfh.Abs_Encoder_fr_id2_oct!=0 and mpfh.Abs_Encoder_fl_id1_oct!=0 and mpfh.Abs_Encoder_rr_id4_oct!=0 and mpfh.Abs_Encoder_rl_id3_oct!=0:
mpfh.New_Read_Encoder_data_From_ABS_Encoder(recevenum)
print "-----Abs_Encoder_fr_id2_oct",mpfh.Abs_Encoder_fr_id2_oct
print "-----Abs_Encoder_fl_id1_oct",mpfh.Abs_Encoder_fl_id1_oct
print "-----Abs_Encoder_rl_id3_oct",mpfh.Abs_Encoder_rl_id3_oct
print "-----Abs_Encoder_rr_id4_oct",mpfh.Abs_Encoder_rr_id4_oct
if flag_1==1:
if flag_fl == 'fl':
fl_error=mpfh.fl_abs_encode-mpfh.Abs_Encoder_fl_id1_oct
print "-----fl error----",fl_error
output_fl.append(fl_error)
if len(output_fl)>3:
velocity_control=P*(output_fl[count_num_fl]-output_fl[count_num_fl-1])+I*output_fl[count_num_fl]+D*(output_fl[count_num_fl]-2*output_fl[count_num_fl-1]+output_fl[count_num_fl-2])
out_vel=(velocity_control*60*50)/1024
print "out_vel-----fl",out_vel
if abs(out_vel)>800 and out_vel<0:
mpfh.MobileControl.Send_Velocity_Driver(int(-1500),'left',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn1'])
print "count num",count_num_fl
elif abs(out_vel)>800 and out_vel>0:
mpfh.MobileControl.Send_Velocity_Driver(int(1500),'left',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn1'])
print "count num",count_num_fl
else:
mpfh.MobileControl.Send_Velocity_Driver(int(out_vel),'left',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn1'])
print "count num",count_num_fl
if abs(fl_error)<=mpfh.MobileControl.CanAnalysis.yamlDic['Homing_error_limit']:
status_ok_fl+=1
if status_ok_fl>=3:
mpfh.MobileControl.Send_Velocity_Driver(0,'left',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn1'])
flag_fl=0
# count_num_fl=0
output_fl=[]
homing_ok_dic.update({'fl':1})
count_num_fl+=1
if flag_fr == 'fr':
fr_error=mpfh.fr_abs_encode-mpfh.Abs_Encoder_fr_id2_oct
print "-----fr error----",fr_error
output_fr.append(fr_error)
if len(output_fr)>3:
velocity_control=P*(output_fr[count_num_fr]-output_fr[count_num_fr-1])+I*output_fr[count_num_fr]+D*(output_fr[count_num_fr]-2*output_fr[count_num_fr-1]+output_fr[count_num_fr-2])
out_vel=(velocity_control*60*50)/1024
print "out_vel-----fr",out_vel
if abs(out_vel)>800 and out_vel<0:
mpfh.MobileControl.Send_Velocity_Driver(int(-1500),'right',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn1'])
print "count num",count_num_fr
elif abs(out_vel)>800 and out_vel>0:
mpfh.MobileControl.Send_Velocity_Driver(int(1500),'right',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn1'])
print "count num",count_num_fr
else:
mpfh.MobileControl.Send_Velocity_Driver(int(out_vel),'right',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn1'])
print "count num",count_num_fr
if abs(fr_error)<=mpfh.MobileControl.CanAnalysis.yamlDic['Homing_error_limit']:
status_ok_fr+=1
if status_ok_fr>=3:
mpfh.MobileControl.Send_Velocity_Driver(0,'right',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn1'])
flag_fr=0
# count_num_fr=0
output_fr=[]
homing_ok_dic.update({'fr':1})
count_num_fr+=1
if flag_rl == 'rl':
rl_error=mpfh.rl_abs_encode-mpfh.Abs_Encoder_rl_id3_oct
print "-----rl error----",rl_error
output_rl.append(rl_error)
if len(output_rl)>3:
velocity_control=P*(output_rl[count_num_rl]-output_rl[count_num_rl-1])+I*output_rl[count_num_rl]+D*(output_rl[count_num_rl]-2*output_rl[count_num_rl-1]+output_rl[count_num_rl-2])
out_vel=(velocity_control*60*50)/1024
print "out_vel-----rl",out_vel
if abs(out_vel)>800 and out_vel<0:
mpfh.MobileControl.Send_Velocity_Driver(int(-1500),'left',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn2'])
print "count num",count_num_rl
elif abs(out_vel)>800 and out_vel>0:
mpfh.MobileControl.Send_Velocity_Driver(int(1500),'left',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn2'])
print "count num",count_num_rl
else:
mpfh.MobileControl.Send_Velocity_Driver(int(out_vel),'left',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn2'])
print "count num",count_num_rl
if abs(rl_error)<=mpfh.MobileControl.CanAnalysis.yamlDic['Homing_error_limit']:
status_ok_rl+=1
if status_ok_rl>=3:
mpfh.MobileControl.Send_Velocity_Driver(0,'left',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn2'])
flag_rl=0
# count_num_rl=0
output_rl=[]
homing_ok_dic.update({'rl':1})
count_num_rl+=1
if flag_rr == 'rr':
rr_error=mpfh.rr_abs_encode-mpfh.Abs_Encoder_rr_id4_oct
print "-----rr error----",rr_error
output_rr.append(rr_error)
if len(output_rr)>3:
velocity_control=P*(output_rr[count_num_rr]-output_rr[count_num_rr-1])+I*output_rr[count_num_rr]+D*(output_rr[count_num_rr]-2*output_rr[count_num_rr-1]+output_rr[count_num_rr-2])
out_vel=(velocity_control*60*50)/1024
print "out_vel-----rr",out_vel
if abs(out_vel)>800 and out_vel<0:
mpfh.MobileControl.Send_Velocity_Driver(int(-1500),'right',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn2'])
print "count num",count_num_rr
elif abs(out_vel)>800 and out_vel>0:
mpfh.MobileControl.Send_Velocity_Driver(int(1500),'right',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn2'])
print "count num",count_num_rr
else:
mpfh.MobileControl.Send_Velocity_Driver(int(out_vel),'right',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn2'])
print "count num",count_num_rr
if abs(rr_error)<=mpfh.MobileControl.CanAnalysis.yamlDic['Homing_error_limit']+1:
status_ok_rr+=1
if status_ok_rr>=3:
mpfh.MobileControl.Send_Velocity_Driver(0,'right',mpfh.MobileControl.CanAnalysis.yamlDic['steering_channel']['chn2'])
flag_rr=0
# count_num_rr=0
output_rr=[]
homing_ok_dic.update({'rr':1})
count_num_rr+=1
if homing_ok_dic.has_key('fl') and homing_ok_dic.has_key('fr') and homing_ok_dic.has_key('rl') and homing_ok_dic.has_key('rr'):
if homing_ok_dic['fl']==1 and homing_ok_dic['fr']==1 and homing_ok_dic['rl']==1 and homing_ok_dic['rr']==1:
flag_1=0
mpfh.MobileControl.Save_Parameter(1)
mpfh.MobileControl.Disable_ALL_Motor_Controller()
print "---------------homing is ok------------------------"
time.sleep(3)
mpfh.MobileControl.Save_Parameter(1)
mpfh.MobileControl.CanAnalysis.Can_VCICloseDevice()
print "---------------homing is ok------------------------"
status_ok_fl=0
status_ok_fr=0
status_ok_rl=0
status_ok_rr=0
# if mpfh.close_homing_flag:
# mpfh.MobileControl.CanAnalysis.Can_VCICloseDevice()
# pubarray=[mpfh.Driver_steer_encode_fl,mpfh.Driver_steer_encode_fr,mpfh.Driver_steer_encode_rl,mpfh.Driver_steer_encode_rr]
# positiondata_array_for_publishing = Int64MultiArray(data=pubarray)
# mpfh.homing_ok_pub.publish(True)
# mpfh.driver_position_feedback_pub.publish(positiondata_array_for_publishing)
else:
mpfh.New_Read_Encoder_data_From_ABS_Encoder(recevenum)
print "read data"
if homing_ok_dic.has_key('fl') and homing_ok_dic.has_key('fr') and homing_ok_dic.has_key('rl') and homing_ok_dic.has_key('rr'):
if homing_ok_dic['fl']==1 and homing_ok_dic['fr']==1 and homing_ok_dic['rl']==1 and homing_ok_dic['rr']==1:
if mpfh.close_homing_flag:
# print "---------------homing is ok------------------------"
# rospy.logerr("---------------homing is ok------------------------")
# mpfh.MobileControl.CanAnalysis.Can_VCICloseDevice()
pubarray=[mpfh.Driver_steer_encode_fl,mpfh.Driver_steer_encode_fr,mpfh.Driver_steer_encode_rl,mpfh.Driver_steer_encode_rr]
positiondata_array_for_publishing = Int64MultiArray(data=pubarray)
mpfh.homing_ok_pub.publish(True)
mpfh.driver_position_feedback_pub.publish(positiondata_array_for_publishing)
# time.sleep(0.1)
elif mpfh.close_homing_flag==False:
print "close uploading data to topics-------------"
else:
pass
rate.sleep()
def obstacles_talker():
rospy.init_node('obstacles_talker')
obstacles_publisher = rospy.Publisher('/path/obstacles',
Int64MultiArray,
queue_size=10)
number_obstacles_publisher = rospy.Publisher('/path/number_obstacles',
Int64MultiArray,
queue_size=10)
lim_obstacles_publisher = rospy.Publisher('/path/lim_obstacles',
Int64MultiArray,
queue_size=10)
path_planing_param_publisher = rospy.Publisher('/path/planing_param',
Int64MultiArray,
queue_size=10)
start_point_publisher = rospy.Publisher('/start_point/position',
Point,
queue_size=10)
goal_point_publisher = rospy.Publisher('/goal_point/position',
Point,
queue_size=10)
#max_iter_publisher = rospy.Publisher('/path/max_iter', Int64MultiArray, queue_size=10)
#step_size_publisher = rospy.Publisher('/path/step_size', Int64MultiArray, queue_size=10)
#goal_reach_thresh_publisher = rospy.Publisher('/path/goal_reach_thresh', Int64MultiArray, queue_size=10)
#drone_radius_publisher = rospy.Publisher('/path/drone_radius', Int64MultiArray, queue_size=10)
obstacles = Int64MultiArray()
number_obstacles = Int64MultiArray()
lim_obstacles = Int64MultiArray()
path_planing_param = Int64MultiArray()
start_point = Point()
goal_point = Point()
#max_iter = Int64MultiArray()
#step_size = Int64MultiArray()
#goal_reach_thresh = Int64MultiArray
#drone_radius = Int64MultiArray()
rate = rospy.Rate(10) # 10hz
while not rospy.is_shutdown():
start_point.x = input("Input start x point: ")
start_point.y = input("Input start y point: ")
goal_point.x = input("Input goal x point: ")
goal_point.y = input("Input goal y point: ")
number = input("Input number obstacles: ")
print("Input plot limit")
_xlim = input("Input -x: ")
xlim = input("Input +x: ")
_ylim = input("Input -y: ")
ylim = input("Input +y: ")
number_obstacles.data = [number]
number_obstacles_publisher.publish(number_obstacles)
lim_obstacles.data = [_xlim, xlim, _ylim, ylim]
lim_obstacles_publisher.publish(lim_obstacles)
s_aria = (abs(_xlim) + abs(xlim)) * (abs(_ylim) + abs(ylim))
max_iter = 50000
step_size = 400
goal_reach_thresh = 1000
#goal_reach_thresh = math.sqrt(s_aria)*0.25
drone_radius = 2
path_planing_param.data = [
max_iter, step_size, goal_reach_thresh, drone_radius
]
path_planing_param_publisher.publish(path_planing_param)
start_point_publisher.publish(start_point)
goal_point_publisher.publish(goal_point)
for i in range(number):
x = input("Input x coor: ")
y = input("Input y coor: ")
z = input("Input radius coor: ")
obstacles.data = [x, y, z]
obstacles_publisher.publish(obstacles)
rate.sleep()
#!/usr/bin/env python
# @file geometry_grounding.py
# @brief This node transforms a command into two x,y coordinates.
import numpy as np
import random
import rospy
import numpy as np
from std_msgs.msg import String
from std_msgs.msg import Int64MultiArray
pub = rospy.Publisher('target_pos', Int64MultiArray, queue_size=10)
pos_to_send = Int64MultiArray()
pos_to_send.data = []
# Callback function for the user command.
# If the command is a "go to x y" command, it sets the target position as x,y.
# If the command is a "go home" command, it sets the target postion as home_posx,home_posy.
# If the command is a "go rand" command, it sets the target position as random coordinates.
# It then publishes the target position.
def callback(data):
input_string = str(data.data)
# Save positions in the command, if any
my_command = [int(s) for s in input_string.split() if s.isdigit()]
def spool_angle_publisher():
global entry_array
pub = rospy.Publisher('spool_encoder', Int64MultiArray, queue_size=10)
rospy.init_node('publisher', anonymous=True)
rate = rospy.Rate(1000)
entry_array = Int64MultiArray()
entry_array.data = []
GPIO.setmode(GPIO.BCM)
GPIO.setup((0, 3), GPIO.OUT)
GPIO.setup((5, 7), GPIO.OUT)
GPIO.setup(1, GPIO.IN)
GPIO.setup(6, GPIO.IN)
init_spool_encoders()
sleep(0.2)
while not rospy.is_shutdown():
entry = np.array([0])
init_spool_encoders()
sleep(0.0000005)
start_read()
sleep(5 / 1000000.0)
for i in range(0, 12):
GPIO.output(3, 1)
GPIO.output(7, 1)
sleep(0.000000375)
a = GPIO.input(1)
b = GPIO.input(6)
sleep(0.000000125)
GPIO.output(3, 0)
GPIO.output(7, 0)
sleep(0.0000005)
if i == 0:
#entry[0] = a
entry_a = str(a)
entry_b = str(b)
else:
#entry = np.append(entry, a)
entry_a = entry_a + str(a)
entry_b = entry_b + str(b)
entry_a = int(entry_a, 2)
entry_b = int(entry_b, 2)
rospy.loginfo(rospy.get_caller_id() + "spool_val_a: " + str(entry_a))
rospy.loginfo(rospy.get_caller_id() + "spool_val_b: " + str(entry_b))
entry_array.data = [entry_a, entry_b]
pub.publish(entry_array)
#rate.sleep()
GPIO.cleanup()
return
class Prob_Updater:
# Subscribers
prob_grids_topic = "/prob_grids"
dirt_grid_topic = "/dirt_occ_grid"
r0_current_vision_topic = "/tb3_0/current_vision"
r1_current_vision_topic = "/tb3_1/current_vision"
# Containers for Subscriber content
# 3D array containing [P,CP,Ex] in the first dimension
prob_grids = Float64MultiArray()
dirt_grid = Int64MultiArray()
r0_current_vision = Int64MultiArray()
r1_current_vision = Int64MultiArray()
# Publisher
pub = rospy.Publisher(prob_grids_topic, Float64MultiArray, queue_size=100)
# time of last update
last_update_time = 0.0
current_time = 0.0
first_time = True
def prob_grid_call_back(self, msg):
self.prob_grids = msg
# rospy.loginfo("Prob_Grid_Updater: New prob grid with dims: %i", len(self.prob_grids.layout.dim))
def dirt_grid_call_back(self, msg):
self.dirt_grid = msg
def r0_current_vision_call_back(self, msg):
self.r0_current_vision = msg
def r1_current_vision_call_back(self, msg):
self.r1_current_vision = msg
def subscribe_to_topics(self):
rospy.Subscriber(self.prob_grids_topic, Float64MultiArray,
self.prob_grid_call_back)
rospy.Subscriber(self.dirt_grid_topic, Int64MultiArray,
self.dirt_grid_call_back)
rospy.Subscriber(self.r0_current_vision_topic, Int64MultiArray,
self.r0_current_vision_call_back)
rospy.Subscriber(self.r1_current_vision_topic, Int64MultiArray,
self.r1_current_vision_call_back)
def publish_to_topics(self, grid):
self.pub.publish(grid)
def prob_index(self, grid, row, col):
pg_rows = self.prob_grids.layout.dim[1].size
pg_cols = self.prob_grids.layout.dim[2].size
return grid * pg_rows * pg_cols + row * pg_cols + col
def update_prob_grid(self):
# Should be executed second
# Executed only for gridpoints in vision
new_list = []
for i in range(0, self.prob_grids.layout.dim[1].size):
for j in range(0, self.prob_grids.layout.dim[2].size):
# if in vision
if (self.r0_current_vision.data[
i * self.r0_current_vision.layout.dim[0].size + j] == 1
or self.r1_current_vision.data[
i * self.r1_current_vision.layout.dim[0].size + j]
== 1):
# if dirt
if self.dirt_grid.data[
i * self.dirt_grid.layout.dim[0].size + j] != 0:
new_list.append(self.dirt_grid.data[
i * self.dirt_grid.layout.dim[0].size + j] /
(self.current_time)) # + 1000*10))
else:
if self.prob_grids.data[self.prob_index(2, i,
j)] > 1.0:
new_list.append(
self.prob_grids.data[self.prob_index(0, i,
j)] / 2)
else:
new_list.append(
self.prob_grids.data[self.prob_index(0, i, j)])
else:
new_list.append(self.prob_grids.data[self.prob_index(
0, i, j)])
return new_list
def update_cp_grid(self):
# Should be executed last
# Executed only for gridpoints in vision
new_list = []
for i in range(0, self.prob_grids.layout.dim[1].size):
for j in range(0, self.prob_grids.layout.dim[2].size):
if (self.r0_current_vision.data[
i * self.r0_current_vision.layout.dim[0].size + j] == 1
or self.r1_current_vision.data[
i * self.r1_current_vision.layout.dim[0].size + j]
== 1):
new_list.append(0)
else:
new_list.append(
1 -
(1 - self.prob_grids.data[self.prob_index(0, i, j)]) *
(1 - self.prob_grids.data[self.prob_index(1, i, j)]))
return new_list
def update_exp_grid(self):
# Should be executed first
# Executed for all gridpoints
new_list = []
diff = self.current_time - self.last_update_time
for i in range(0, self.r0_current_vision.layout.dim[0].size):
for j in range(0, self.r0_current_vision.layout.dim[1].size):
# if inside vision
if (self.r0_current_vision.data[
i * self.r0_current_vision.layout.dim[0].size + j] == 1
or self.r1_current_vision.data[
i * self.r1_current_vision.layout.dim[0].size + j]
== 1):
# if expectation greater than 1
if (self.prob_grids.data[self.prob_index(2, i, j)]) > 1.:
new_list.append(0)
else:
new_list.append(self.prob_grids.data[self.prob_index(
2, i, j)])
else:
new_list.append(
self.prob_grids.data[self.prob_index(2, i, j)] +
self.prob_grids.data[self.prob_index(0, i, j)] * diff)
return new_list
def spin(self):
# Prob. Grid initialisation constant
prob_init_const = 0.01
rospy.loginfo("Prob_Grid_Updater: Starting Spin function")
r = rospy.Rate(1)
while not rospy.is_shutdown():
cv_xdim = self.r0_current_vision.layout.dim[1].size
cv_ydim = self.r0_current_vision.layout.dim[0].size
new_prob_grids = Float64MultiArray()
new_prob_grids.layout.dim.extend([
MultiArrayDimension(),
MultiArrayDimension(),
MultiArrayDimension()
])
new_prob_grids.layout.dim[0].size = 3
new_prob_grids.layout.dim[0].label = "grids"
new_prob_grids.layout.dim[1].label = "rows"
new_prob_grids.layout.dim[1].size = cv_ydim
new_prob_grids.layout.dim[2].label = "cols"
new_prob_grids.layout.dim[2].size = cv_xdim
new_prob_grids.data = []
# Assign new / old times
if self.first_time == True:
rospy.loginfo(
"Prob_Grid_Updater: First time so initialising prob grids")
self.last_update_time = rospy.get_time()
# init the CP grids
# data initialisations
# Expected and CP grid uniformly as 0
for i in range(0, 3):
for j in range(0, cv_ydim):
for k in range(0, cv_xdim):
if (i == 0):
new_prob_grids.data.append(prob_init_const)
else:
new_prob_grids.data.append(0)
self.first_time = False
else:
self.current_time = rospy.get_time()
# rospy.loginfo("Prob_Grid_Updater: Current Time = %i", self.current_time)
expected_list = self.update_exp_grid()
prob_list = self.update_prob_grid()
cp_list = self.update_cp_grid()
new_prob_grids.data.extend(prob_list)
new_prob_grids.data.extend(cp_list)
new_prob_grids.data.extend(expected_list)
# self.print_list(prob_list,20)
# print(" ")
# self.print_list(expected_list, 20)
# print(" ")
# self.print_list(cp_list, 20)
self.publish_to_topics(new_prob_grids)
self.last_update_time = self.current_time
# rospy.loginfo('Updated Prob_Grids')
r.sleep()
def print_list(self, some_list, width):
np.set_printoptions(precision=3)
for i in range(0, len(some_list) / width):
a = [
round(n + 0.001, 3)
for n in some_list[i * width:(i + 1) * width]
]
print(a)
#!/usr/bin/env python
# coding: utf-8
#======= Import ================
import rospy
from std_msgs.msg import Float64MultiArray
from std_msgs.msg import Int64MultiArray
from sensor_msgs.msg import Range
from sensor_msgs.msg import BatteryState
from tquad.msg import LineSensor
lineSensor = LineSensor()
ultrasound = Range()
battery = BatteryState()
encoders = Int64MultiArray()
def publishRange(value):
"""
Fonction pour publier la valeur renvoyée par le capteur ultrason
"""
ultrasound.header.stamp = rospy.Time.now()
ultrasound.header.frame_id = "/ultrasound"
ultrasound.radiation_type = 0
ultrasound.field_of_view = 0.1
ultrasound.min_range = 0
ultrasound.max_range = 2
ultrasound.range = value
ultrasound_pub.publish(ultrasound)