def SetLocalLogger(self, allowLocalLogging, localFileName):
self.logger.SetLocalLogger(self._to_cpp(Bool(allowLocalLogging)),
self._to_cpp(String(localFileName)))
def Running(self):
reset_msg = Bool()
reset_msg.data = True
self.resetPub.publish(reset_msg)
rospy.sleep(1)
self.odom.header.frame_id = "odom"
self.odom.child_frame_id = "base_footprint"
self.odom.pose.pose.position.z = 0
while not rospy.is_shutdown():
# Calculate robot postion
deltaLeft = self.left_count - self.last_left
deltaRight = self.right_count - self.last_right
delta_time = float(str(rospy.Time.now() - self.time))/1000000000
self.last_left = self.left_count
self.last_right = self.right_count
if math.fabs(deltaLeft) > 10000 or math.fabs(deltaRight) > 10000:
continue
if self.left_count == 0 and self.right_count ==0:
if math.fabs(deltaLeft) > 0 or math.fabs(deltaRight) > 0:
continue
deltaDistance = (deltaLeft + deltaRight) * self.distance_per_count / 2
deltaAngle = (deltaRight - deltaLeft) * self.distance_per_count / self.track_width
self.x += deltaDistance * math.cos(self.pose.theta)
self.y += deltaDistance * math.sin(self.pose.theta)
self.theta += deltaAngle
self.v = (self.left_speed + self.right_speed) / 2
self.w = (self.right_speed - self.left_speed) / self.track_width
# Publish odometry
quaternion = Quaternion()
quaternion.x = 0.0
quaternion.y = 0.0
quaternion.z = sin(self.theta / 2.0)
quaternion.w = cos(self.theta / 2.0)
ros_now = rospy.Time.now()
self._OdometryTransformBroadcaster.sendTransform(
(self.x, self.y, 0),
(quaternion.x, quaternion.y, quaternion.z, quaternion.w),
ros_now,
"base_footprint",
"odom"
)
odometry = Odometry()
odometry.header.frame_id = "odom"
odometry.header.stamp = ros_now
odometry.pose.pose.position.x = self.x
odometry.pose.pose.position.y = self.y
odometry.pose.pose.position.z = 0
odometry.pose.pose.orientation = quaternion
odometry.child_frame_id = "base_link"
odometry.twist.twist.linear.x = self.v
odometry.twist.twist.linear.y = 0
odometry.twist.twist.angular.z = self.w
self._OdometryPublisher.publish(odometry)
# Set the frame and position for the odometry
odometry = odo
odometry.header.frame_id = "odom"
odometry.header.stamp = ros_now
odometry.child_frame_id = "zed"
odometry.pose.pose.position.z = 0
self.rate.sleep()
def main(args):
global wait_measurement_sensor_flag
global wait_measurement_pos_x_flag
global wait_measurement_pos_y_flag
global current_position
global current_state
global polygon_voronoi_publisher
global list_current_position, poses
global list_current_velocity
global local_vel_pub
global local_pos_pub
global init_coverage_start
global mission_state
global prev_position
global mission_state_pub, RVO_pub_list, rviz_visualization_start
global uav_marker,uav_marker_publisher,uav_goal_marker,uav_goal_marker_publisher,br,uav_idx,uav_ID
#thread.join()
par = lp.Parameter
re = les.RecursiveEstimation
true_function = lp.TrueFunction(par)
est_function = re.DensityFunction
ne = les.NoEstimation
par.CurrentIteration = 1
#print "Program Started"
dir_path = os.path.dirname(os.path.realpath(__file__))
print dir_path
with open(dir_path+'/config.json','r') as f:
config = json.load(f)
uav_ID = "1"
x_val = 0
y_val = 0
z_val = 0
for i in range(len(args)):
if(args[i] == "-uav"):
uav_ID = args[i+1]
elif(args[i] == "-x"):
x_val = float(args[i+1])
elif(args[i] == "-y"):
y_val = float(args[i+1])
elif(args[i] == "-z"):
z_val = float(args[i+1])
elif(args[i] == "-rwd"):
rwd_val = args[i+1]
init_pos = config["UAV"][uav_ID]["INIT_POS"]
uav_color = config["UAV"][uav_ID]["COLOR"]
print "init pos: ",init_pos
rospy.init_node("control_uav_"+uav_ID)
uav_idx = int(uav_ID)-1
br = tf.TransformBroadcaster()
publish_uav_position_rviz(br,init_pos[0],init_pos[1],init_pos[2],uav_ID)
pathname=os.path.abspath(rwd_val+'/src/coverage_control_uavs/control_uav/scripts/')
print pathname
listfolder = []
for foldername in os.listdir(pathname+'/img_res'):
listfolder.append(int(foldername))
#if fname.endswith('.mp4'):
# listvideo.append(fname)
if(len(listfolder) == 0):
curr_folder = 0
else:
curr_folder = np.max(listfolder)
print 'current folder:',curr_folder
folderstring = pathname+'/img_res/%04d/'%curr_folder
if(uav_ID == '1'):
os.system("cp -i "+dir_path+'/config.json '+folderstring)
print "cp -i "+dir_path+'/config.json '+folderstring
try:
rospy.wait_for_service("/uav"+uav_ID+"/mavros/cmd/arming")
rospy.wait_for_service("/uav"+uav_ID+"/mavros/set_mode")
except rospy.ROSException:
fail("failed to connect to service")
state_sub=rospy.Subscriber("/uav"+uav_ID+"/mavros/state", State,queue_size=10,callback=state_cb)
#velocity_sub=rospy.Subscriber("/uav"+uav_ID+"/mavros/local_position/velocity",)
#list_UAV_ID,num_agent = get_number_of_agent()
#num_agent = len(config["UAV"])
list_UAV_ID = []
all_init_pos = []
for i in range(par.NumberOfAgents):
list_UAV_ID.append("uav"+repr(i+1))
all_init_pos.append(config["UAV"][repr(i+1)]["INIT_POS"])
print "NUM OF AGENT = ",par.NumberOfAgents
print "LIST UAV = ",list_UAV_ID
do_coverage_flag = Bool()
do_coverage_flag.data = False
reach_goal_flag = Bool()
reach_goal_flag.data = True
var_float = Float32()
var_float.data = 0.0
for i in range(par.NumberOfAgents):
list_current_position.append(PoseStamped())
list_current_velocity.append(TwistStamped())
list_reach_goal.append(reach_goal_flag)
list_do_coverage.append(do_coverage_flag)
#list_measurement_sensor.append(var_float)
list_measurement_sensor.append(0.0)
wait_measurement_sensor_flag.append(0.0)
#list_measurement_pos_x.append(var_float)
list_measurement_pos_x.append(0.0)
wait_measurement_pos_x_flag.append(0.0)
#list_measurement_pos_y.append(var_float)
list_measurement_pos_y.append(0.0)
wait_measurement_pos_y_flag.append(0.0)
V_max = []
for i in range(par.NumberOfAgents):
velocity_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb, callback_args=i)
position_sub = rospy.Subscriber("/uav"+repr(i+1)+"/mavros/local_position/pose",PoseStamped,queue_size = 10,callback=position_cb,callback_args=(i,uav_idx,all_init_pos))
reach_goal_sub = rospy.Subscriber("/uav"+repr(i+1)+"/reach_goal",Bool,queue_size=10,callback=reach_goal_cb,callback_args=i)
do_coverage_sub = rospy.Subscriber("/uav"+repr(i+1)+"/do_coverage",Bool,queue_size=10,callback=do_coverage_cb,callback_args=i)
measurement_sensor_sub = rospy.Subscriber("/uav"+repr(i+1)+"/measurement_sensor",Float32MultiArray,queue_size=10,callback=measurement_sensor_cb,callback_args=i)
measurement_pos_x_sub = rospy.Subscriber("/uav"+repr(i+1)+"/measurement_pos_x",Float32MultiArray,queue_size=10,callback=measurement_pos_x_cb,callback_args=i)
measurement_pos_y_sub = rospy.Subscriber("/uav"+repr(i+1)+"/measurement_pos_y",Float32MultiArray,queue_size=10,callback=measurement_pos_y_cb,callback_args=i)
V_max.append(par.V_MAX)
RVO_pub_list = []
for i in range(len(par.ws_model['circular_obstacles'])):
RVO_pub_list.append(rospy.Publisher("/uav"+uav_ID+"/RVO"+repr(i),Float32MultiArray,queue_size=10))
scan_lidar_sub = rospy.Subscriber("/scan",LaserScan,queue_size=10,callback=scan_lidar_cb,callback_args=uav_ID)
#velocity_sub = rospy.Subscriber("/uav1/mavros/setpoint_raw/global",GlobalPositionTarget,queue_size = 10,callback=velocity_cb)
#velocity_sub = rospy.Subscriber("/uav1/mavros/setpoint_raw/global",PositionTarget,queue_size = 10,callback=velocity_cb)
#velocity_sub = rospy.Subscriber("/uav1/mavros/local_position/velocity",TwistStamped,queue_size = 10,callback=velocity_cb)
#position_sub = rospy.Subscriber("/uav1/mavros/local_position/pose",PoseStamped,queue_size = 10,callback=position_cb)
global local_pos_pub, poses
local_pos_pub=rospy.Publisher("/uav"+uav_ID+"/mavros/setpoint_position/local",PoseStamped,queue_size=10)
local_vel_pub=rospy.Publisher("/uav"+uav_ID+"/mavros/setpoint_velocity/cmd_vel",TwistStamped,queue_size=10)
goal_pos_pub=rospy.Publisher("/uav"+uav_ID+"/goal_position",PoseStamped,queue_size=10)
sub_goal_pos_pub=rospy.Publisher("/uav"+uav_ID+"/sub_goal_position",PoseStamped,queue_size=10)
local_reach_goal_flag_pub=rospy.Publisher("/uav"+uav_ID+"/reach_goal",Bool,queue_size=10)
local_do_coverage_flag_pub=rospy.Publisher("/uav"+uav_ID+"/do_coverage",Bool,queue_size=10)
mission_state_pub=rospy.Publisher("/uav"+uav_ID+"/mission_state",Int32,queue_size=10)
#local_measurement_sensor_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_sensor",Float32,queue_size=10)
#local_measurement_pos_x_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_pos_x",Float32,queue_size=10)
#local_measurement_pos_y_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_pos_y",Float32,queue_size=10)
local_measurement_sensor_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_sensor",Float32MultiArray,queue_size=10)
local_measurement_pos_x_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_pos_x",Float32MultiArray,queue_size=10)
local_measurement_pos_y_pub=rospy.Publisher("/uav"+uav_ID+"/measurement_pos_y",Float32MultiArray,queue_size=10)
polygon_voronoi_publisher = rospy.Publisher("/uav"+uav_ID+"/polygon_voronoi",Polygon,queue_size=10)
voronoi_grid_publisher = rospy.Publisher("/uav"+uav_ID+"/voronoi_grid",Int8MultiArray,queue_size=10)
current_iteration_publisher = rospy.Publisher("/current_iteration",Int32,queue_size=10)
voronoi_grid = Int8MultiArray()
voronoi_grid.layout.dim.append(MultiArrayDimension())
voronoi_grid.layout.dim.append(MultiArrayDimension())
voronoi_grid.layout.dim[0].label = "height"
voronoi_grid.layout.dim[1].label = "width"
voronoi_grid.layout.dim[0].size = par.NumberOfPoints
voronoi_grid.layout.dim[1].size = par.NumberOfPoints
voronoi_grid.layout.dim[0].stride = par.NumberOfPoints*par.NumberOfPoints
voronoi_grid.layout.dim[1].stride = par.NumberOfPoints
voronoi_grid.layout.data_offset = 0
voronoi_grid.data = np.zeros((par.NumberOfPoints,par.NumberOfPoints)).reshape((1,par.NumberOfPoints*par.NumberOfPoints)).tolist()[0]
polygon_msg = Polygon()
meas_sensor = Float32MultiArray()
meas_sensor.layout.dim.append(MultiArrayDimension())
meas_sensor.layout.dim[0].label = "length"
meas_sensor.layout.dim[0].size = 2
meas_sensor.layout.dim[0].stride = 2
meas_sensor.layout.data_offset = 0
meas_sensor.data = [0.0,0.0]
meas_pos_x = Float32MultiArray()
meas_pos_x.layout.dim.append(MultiArrayDimension())
meas_pos_x.layout.dim[0].label = "length"
meas_pos_x.layout.dim[0].size = 2
meas_pos_x.layout.dim[0].stride = 2
meas_pos_x.layout.data_offset = 0
meas_pos_x.data = [0.0,0.0]
meas_pos_y = Float32MultiArray()
meas_pos_y.layout.dim.append(MultiArrayDimension())
meas_pos_y.layout.dim[0].label = "length"
meas_pos_y.layout.dim[0].size = 2
meas_pos_y.layout.dim[0].stride = 2
meas_pos_y.layout.data_offset = 0
meas_pos_y.data = [0.0,0.0]
est_function_pc = Float32MultiArray()
est_function_pc.layout.dim.append(MultiArrayDimension())
est_function_pc.layout.dim.append(MultiArrayDimension())
est_function_pc.layout.dim[0].label = "height"
est_function_pc.layout.dim[1].label = "width"
est_function_pc.layout.dim[0].size = est_function.shape[0]
est_function_pc.layout.dim[1].size = est_function.shape[1]
est_function_pc.layout.dim[0].stride = est_function.shape[0]*est_function.shape[1]
est_function_pc.layout.dim[1].stride = est_function.shape[1]
est_function_pc.layout.data_offset = 0
est_function_pc.data = est_function.reshape((1,est_function.shape[0]*est_function.shape[1])).tolist()[0]
est_sensory_function_pub=rospy.Publisher("/uav"+uav_ID+"/est_sensory_function",Float32MultiArray,queue_size=10)
arming_client = rospy.ServiceProxy("/uav"+uav_ID+"/mavros/cmd/arming",CommandBool)
set_mode_client = rospy.ServiceProxy("/uav"+uav_ID+"/mavros/set_mode",SetMode)
topic = 'uav_marker_'+uav_ID
uav_marker_publisher = rospy.Publisher(topic, Marker,queue_size=10)
uav_marker = Marker()
uav_marker.header.frame_id = "world"
uav_marker.type = uav_marker.SPHERE
uav_marker.action = uav_marker.ADD
uav_marker.scale.x = par.ws_model['robot_radius']*3
uav_marker.scale.y = par.ws_model['robot_radius']*3
uav_marker.scale.z = 0.005*par.RealScale
uav_marker.color.r = float(uav_color[0])/255
uav_marker.color.g = float(uav_color[1])/255
uav_marker.color.b = float(uav_color[2])/255
uav_marker.color.a = float(uav_color[3])/255
uav_marker.pose.orientation.w = 1.0
uav_marker.pose.position.x = init_pos[0]
uav_marker.pose.position.y = init_pos[1]
uav_marker.pose.position.z = init_pos[2]
uav_marker_publisher.publish(uav_marker)
poses = PoseStamped()
#poses.pose = Pose()
#poses.pose.position = Point()
poses.pose.position.x = x_val
poses.pose.position.y = y_val
poses.pose.position.z = z_val
uav_color = [255,255,255,255]
topic = 'uav_goal_marker_'+uav_ID
uav_goal_marker_publisher = rospy.Publisher(topic, Marker,queue_size=10)
uav_goal_marker = Marker()
uav_goal_marker.header.frame_id = "world"
uav_goal_marker.type = uav_goal_marker.CUBE
uav_goal_marker.action = uav_goal_marker.ADD
uav_goal_marker.scale.x = par.ws_model['robot_radius']*2
uav_goal_marker.scale.y = par.ws_model['robot_radius']*2
uav_goal_marker.scale.z = 0.005*par.RealScale
uav_goal_marker.color.r = float(uav_color[0])/255
uav_goal_marker.color.g = float(uav_color[1])/255
uav_goal_marker.color.b = float(uav_color[2])/255
uav_goal_marker.color.a = float(uav_color[3])/255
uav_goal_marker.pose.orientation.w = 1.0
uav_goal_marker.pose.position.x = 0#init_pos[0]
uav_goal_marker.pose.position.y = 0#init_pos[1]
uav_goal_marker.pose.position.z = 0#init_pos[2]
uav_goal_marker_publisher.publish(uav_goal_marker)
uav_goal_marker=update_uav_marker(uav_goal_marker,(poses.pose.position.x,poses.pose.position.y,poses.pose.position.z,1.0))
uav_goal_marker_publisher.publish(uav_goal_marker)
r=rospy.Rate(10)
vController = VelocityController()
#print(dir(current_state.connected))
print "TRY TO CONNECT"
while ((not rospy.is_shutdown()) and (not current_state.connected)):
#rospy.spinOnce()
r.sleep()
print "CONNECTED"
#print(current_state.connected.__class__)
rospy.loginfo("CURRENT STATE CONNECTED")
init_position_coverage = (x_val,y_val,z_val)
print poses.pose.position.x,poses.pose.position.y,poses.pose.position.z
i = 100
while((not rospy.is_shutdown()) and (i>0)):
local_pos_pub.publish(poses)
i = i-1
rospy.loginfo("POSITION PUBLISHED")
publish_mission_state(mission_state)
thread1 = Thread(target = pose_pub_function)
thread1.start()
thread2 = Thread(target = velocity_pub_function,args=(par,int(uav_ID)-1,vController))
thread2.start()
thread3 = Thread(target = error_pub_function,args=(par,int(uav_ID)-1))
thread3.start()
last_request = rospy.Time.now()
last_request_neighbor = rospy.Time.now()
last_request_rviz = rospy.Time.now()
last_request_debug = rospy.Time.now()
last_iteration_timeout = rospy.Time.now()
rviz_visualization_start = False
coverage_start = False
coverage_end = False
averageErrorDensityFunction = []
timePerIteration = []
timeCoverage = []
while(not rospy.is_shutdown()):
#offb_set_mode = set_mode_client(0,'OFFBOARD')
if(rviz_visualization_start and (rospy.Time.now()-last_request_rviz > rospy.Duration(0.2))):
#publish_uav_position_rviz(br,list_current_position[uav_idx].pose.position.x,list_current_position[uav_idx].pose.position.y,list_current_position[uav_idx].pose.position.z,uav_ID)
#uav_marker=update_uav_marker(uav_marker,(list_current_position[uav_idx].pose.position.x,list_current_position[uav_idx].pose.position.y,list_current_position[uav_idx].pose.position.z,1.0))
#uav_marker_publisher.publish(uav_marker)
#uav_goal_marker=update_uav_marker(uav_goal_marker,(poses.pose.position.x,poses.pose.position.y,poses.pose.position.z,1.0))
#uav_goal_marker_publisher.publish(uav_goal_marker)
last_request_rviz = rospy.Time.now()
#print "list_reach_goal",list_reach_goal
if(rviz_visualization_start and (rospy.Time.now()-last_request_debug > rospy.Duration(1.0))):
last_request_debug = rospy.Time.now()
#print "+++ DEBUG +++"
#print list_do_coverage
#print "+++++++++++++"
if(rospy.Time.now()-last_request_neighbor > rospy.Duration(10.0)):
#for i in range(num_agent):
# print list_current_position[i].pose.position.x,list_current_position[i].pose.position.y
last_request_neighbor = rospy.Time.now()
if((not rviz_visualization_start) and (current_state.mode != 'OFFBOARD') and (rospy.Time.now()-last_request > rospy.Duration(1.0))):
offb_set_mode = set_mode_client(0,'OFFBOARD')
if(offb_set_mode.mode_sent):
rospy.loginfo("OFFBOARD ENABLED")
last_request = rospy.Time.now()
else:
if((not current_state.armed) and (rospy.Time.now()-last_request > rospy.Duration(1.0))):
arm_cmd = arming_client(True)
if(arm_cmd.success):
rospy.loginfo("VEHICLE ARMED")
rviz_visualization_start = True
#coverage_start = True
reach_goal_flag = Bool()
reach_goal_flag.data = True
last_request_rviz = rospy.Time.now()
last_request=rospy.Time.now()
if(par.CurrentIteration == 1) and not coverage_start:
#print "current position", current_position
#print "init pos coverage", init_position_coverage
#if(distance3D(init_position_coverage,current_position)<3):
if(rospy.Time.now()-last_iteration_timeout > rospy.Duration(5.0)):
last_iteration_timeout = rospy.Time.now()
target = Pose()
targetStamped = PoseStamped()
target.position.x = poses.pose.position.x
target.position.y = poses.pose.position.y
target.position.z = poses.pose.position.z
diagram4 = GridWithWeights(par.NumberOfPoints,par.NumberOfPoints)
diagram4.walls = []
for i in range(par.NumberOfPoints): # berikan wall pada algoritma pathfinding A*
for j in range(par.NumberOfPoints):
if (par.ObstacleRegion[j,i]==0):
diagram4.walls.append((i,j))
for i in range(par.NumberOfPoints):
for j in range(par.NumberOfPoints):
if(par.ObstacleRegionWithClearence[j][i] == 0):
diagram4.weights.update({(i,j): par.UniversalCost})
goal_pos = (target.position.x/par.RealScale,target.position.y/par.RealScale)
start_pos = (cur_pose.pose.position.x/par.RealScale,cur_pose.pose.position.y/par.RealScale)
pathx,pathy=Astar_version1(par,start_pos,goal_pos,diagram4)
target.position.x = pathx[0]*par.RealScale
target.position.y = pathy[0]*par.RealScale
vController.setHeadingTarget(deg2rad(45.0))
vController.setTarget(target)
vController.setPIDGainX(1,0.0,0.0)
vController.setPIDGainY(1,0.0,0.0)
vController.setPIDGainZ(1,0,0)
vController.setPIDGainPHI(1,0.0,0.0)
vController.setPIDGainTHETA(1,0.0,0.0)
vController.setPIDGainPSI(1,0.0,0.0)
init_coverage_start = True
coverage_start = True
V_msg = TwistStamped()
V_msg.header.stamp = rospy.Time.now()
V_msg.twist.linear.x =0 #
V_msg.twist.linear.y =0 #
V_msg.twist.linear.z = 0
V_msg.twist.angular.x = 0.0
V_msg.twist.angular.y = 0.0
V_msg.twist.angular.z = 0.0 #w
local_vel_pub.publish(V_msg)
mission_state = AGENT_ARRIVE_AT_GOAL
publish_mission_state(mission_state)
if(coverage_start): # and all(item.data == True for item in list_reach_goal)): # coverage
#gaussian regression
print "time:" , datetime.datetime.now().strftime("%H:%M:%S")
print "do coverage iter-",par.CurrentIteration
do_coverage_flag = Bool()
do_coverage_flag.data = True
x = (list_current_position[uav_idx].pose.position.x/par.RealScale,list_current_position[uav_idx].pose.position.y/par.RealScale)
y = com.transmitNewMeasurementOneAgent(par,x)
#measurement.data = [par.CurrentIteration,x[0],x[1],y]
for i in range(par.NumberOfAgents):
meas_sensor.data = [par.CurrentIteration,y]
meas_pos_x.data = [par.CurrentIteration,x[0]]
meas_pos_y.data = [par.CurrentIteration,x[1]]
local_measurement_sensor_pub.publish(meas_sensor)
local_measurement_pos_x_pub.publish(meas_pos_x)
local_measurement_pos_y_pub.publish(meas_pos_y)
if(par.UsingStrategy == 2):
while not (wait_measurement_sensor_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2):
pass
while not (wait_measurement_pos_x_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2):
pass
while not (wait_measurement_pos_y_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2):
pass
for i in range(par.NumberOfAgents):
if(wait_measurement_sensor_flag[i] == par.CurrentIteration) and (wait_measurement_pos_x_flag[i] == par.CurrentIteration) and (wait_measurement_pos_y_flag[i] == par.CurrentIteration):
re.statusFinishedAgent[i] = 0
if(par.UsingStrategy == 1):
while not all(item == par.CurrentIteration for item in wait_measurement_sensor_flag):
pass
while not all(item == par.CurrentIteration for item in wait_measurement_pos_x_flag):
pass
while not all(item == par.CurrentIteration for item in wait_measurement_pos_y_flag):
pass
#print "wait_meas_sensor",wait_measurement_sensor_flag
#print "wait_meas_pos_x",wait_measurement_pos_x_flag
#print "wait_meas_pos_y",wait_measurement_pos_y_flag
#for i in range(par.NumberOfAgents):
# wait_measurement_sensor_flag[i] = True
# wait_measurement_pos_x_flag[i] = True
# wait_measurement_pos_y_flag[i] = True
print "list_measurement:",list_measurement_sensor,list_measurement_pos_x,list_measurement_pos_y
# wait all agent measure
iteration_msg = Int32()
iteration_msg.data = par.CurrentIteration
if(uav_ID == '1'):
current_iteration_publisher.publish(iteration_msg)
if(uav_ID == '1'):
if(par.CurrentIteration>1):
timePerIteration.append(time.time()-timePerIterationStart)
f= open(folderstring+"/timeiter.txt","a+")
f.write("%f\r\n" % (timePerIteration[par.CurrentIteration-2]))
f.close()
timePerIterationStart = time.time()
timeCoverageStart = time.time()
re = com.joinNewMeasurements(par,re,list_measurement_sensor,list_measurement_pos_x,list_measurement_pos_y)
timeCoverageStart = time.time()
re = com.computeCentralizedEstimation(re,par)
re = com.computeCoverage(re,par,re.APosterioriVariance,par.ws_model)
if(uav_ID == '1'):
timeCoverage.append(time.time()-timeCoverageStart)
selisih = re.DensityFunction - true_function
averageErrorDensityFunction.append(np.sum(np.sum(np.multiply(selisih,selisih)))/(par.NumberOfPoints*par.NumberOfPoints))
re.CostFunction.append(com.computeCost(par,re.Node,re.DensityFunction,par.CostFlag))
f= open(folderstring+"/maxvar.txt","a+")
f.write("%f\r\n" % (re.maxAPosterioriVariance[par.CurrentIteration-1]))
f.close()
f= open(folderstring+"/minvar.txt","a+")
f.write("%f\r\n" % (re.minAPosterioriVariance[par.CurrentIteration-1]))
f.close()
f= open(folderstring+"/avgvar.txt","a+")
f.write("%f\r\n" % (re.averageAPosterioriVariance[par.CurrentIteration-1]))
f.close()
f= open(folderstring+"/avgdens.txt","a+")
f.write("%f\r\n" % (averageErrorDensityFunction[par.CurrentIteration-1]))
f.close()
f= open(folderstring+"/timecoverage.txt","a+")
f.write("%f\r\n" % (timeCoverage[par.CurrentIteration-1]))
f.close()
f= open(folderstring+"/cost.txt","a+")
f.write("%f\r\n" % (re.CostFunction[par.CurrentIteration-1]))
f.close()
f= open(folderstring+"/energy.txt","a+")
f.write("%f\r\n" % (re.averageEnergy[par.CurrentIteration-1]))
f.close()
est_function = re.DensityFunction
est_function_pc.data = est_function.reshape((1,est_function.shape[0]*est_function.shape[1])).tolist()[0]
if(uav_ID == '1'):
est_sensory_function_pub.publish(est_function_pc)
prev_position = (poses.pose.position.x, poses.pose.position.y, poses.pose.position.z)
poses.pose.position.x = re.Node[uav_idx].pos[0]*par.RealScale
poses.pose.position.y = re.Node[uav_idx].pos[1]*par.RealScale
#print poses.pose.position.x,poses.pose.position.y
poses.pose.position.z = par.StaticAltitude
goal_pos_pub.publish(poses)
mission_state = AGENT_GOES_TO_GOAL #AGENT_HOVER_YAWING
publish_mission_state(mission_state)
#
if(control_type == PID_CONTROL):
target = Pose()
target.position.x = poses.pose.position.x
target.position.y = poses.pose.position.y
target.position.z = poses.pose.position.z
vController.setHeadingTarget(deg2rad(45.0))
vController.setTarget(target)
vController.setPIDGainX(1,0.0,0.0)
vController.setPIDGainY(1,0.0,0.0)
vController.setPIDGainZ(1,0,0)
vController.setPIDGainPHI(1,0.0,0.0)
vController.setPIDGainTHETA(1,0.0,0.0)
vController.setPIDGainPSI(1,0.0,0.0)
goal_pos = (target.position.x/par.RealScale,target.position.y/par.RealScale)
start_pos = (cur_pose.pose.position.x/par.RealScale,cur_pose.pose.position.y/par.RealScale)
pathx,pathy=Astar_version1(par,start_pos,goal_pos,diagram4)
target.position.x = pathx[0]*par.RealScale
target.position.y = pathy[0]*par.RealScale
vController.setHeadingTarget(deg2rad(45.0))
vController.setTarget(target)
targetStamped.pose = target
sub_goal_pos_pub.publish(targetStamped)
print "Max Variance [iter:"+repr(par.CurrentIteration)+"] : ",np.max(np.max(re.APosterioriVariance))
publish_polygon_voronoi(re.Node[uav_idx].pvicini,par.RealScale)
voronoi_grid.data = re.Node[uav_idx].VoronoiRegion.reshape((1,par.NumberOfPoints*par.NumberOfPoints)).tolist()[0]
voronoi_grid_publisher.publish(voronoi_grid)
#print "Input Locations: ",re.InputLocations
#reach_goal_flag.data = False
#coverage_end = True
coverage_start = False
par.CurrentIteration = par.CurrentIteration +1
if init_coverage_start:
if(distance2D(cur_pose.pose,target) < 3):
if(len(pathx) > 1):
del pathx[0]
del pathy[0]
target.position.x = pathx[0]*par.RealScale
target.position.y = pathy[0]*par.RealScale
print "change subtarget"
vController.setTarget(target)
targetStamped.pose = target
sub_goal_pos_pub.publish(targetStamped)
if (all(item.data == True for item in list_do_coverage)):
#print "all done coverage"
#print poses.pose.position.x,poses.pose.position.y
#print list_current_position[uav_idx].pose.position.x,list_current_position[uav_idx].pose.position.y
reach_goal_flag = Bool()
reach_goal_flag.data = False
pose_x_diff = (poses.pose.position.x-list_current_position[uav_idx].pose.position.x)**2
pose_y_diff = (poses.pose.position.y-list_current_position[uav_idx].pose.position.y)**2
x = (list_current_position[uav_idx].pose.position.x,list_current_position[uav_idx].pose.position.y,list_current_position[uav_idx].pose.position.z)
goal = (poses.pose.position.x,poses.pose.position.y,poses.pose.position.z)
timeout_flag = rospy.Time.now()-last_iteration_timeout > rospy.Duration(5.0)
if((do_coverage_flag.data == True) and (reach_goal_flag.data == False) and (timeout_flag or ((par.UsingStrategy == 2) and (wait_measurement_sensor_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2)))) :
#if((do_coverage_flag.data == True) and (reach_goal_flag.data == False) and ('''(distance3D(goal,x) < 2)'''timeout_flag or ((par.UsingStrategy == 2) and (wait_measurement_sensor_flag.count(par.CurrentIteration) >= par.NumberOfAgents // 2)))) :
last_iteration_timeout = rospy.Time.now()
mission_state = AGENT_ARRIVE_AT_GOAL
publish_mission_state(mission_state)
print "time:" , datetime.datetime.now().strftime("%H:%M:%S")
print "agent "+uav_ID+" reach the goal"
reach_goal_flag = Bool()
reach_goal_flag.data = True
# Node pos is current position
for i in range(par.NumberOfAgents):
re.Node[i].pos[0]=float(list_current_position[i].pose.position.x)/par.RealScale
re.Node[i].pos[1]=float(list_current_position[i].pose.position.y)/par.RealScale
#coverage_end = False
do_coverage_flag = Bool()
do_coverage_flag.data = False
coverage_start = True
for i in range(par.NumberOfAgents):
local_reach_goal_flag_pub.publish(reach_goal_flag)
while(not all(item.data == True for item in list_reach_goal)):
#local_reach_goal_flag_pub.publish(reach_goal_flag)
pass
print "time:" , datetime.datetime.now().strftime("%H:%M:%S")
print "list_reach_goal",list_reach_goal
# publish everything
local_do_coverage_flag_pub.publish(do_coverage_flag)
local_reach_goal_flag_pub.publish(reach_goal_flag)
#local_pos_pub.publish(poses)
r.sleep()
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
from mobrob_util.msg import ME439WaypointXY, ME439SensorsProcessed
from geometry_msgs.msg import Pose2D
from std_msgs.msg import Bool
# Waypoints to hit: a "numpy.array" of [x,y] coordinates.
# Example: Square
# waypoints = np.array([[0.5, 0.], [0.5, 0.5], [0., 0.5], [0., 0.]])
goal = np.array([0, 2])
##################################################################
# Run the Publisher
##################################################################
# initialize the current "segment" to be the first one (index 0) # (you
# could skip segments if you wanted to)
waypoint_number = 0 # for waypoint seeking.
path_complete = Bool()
path_complete.data = False
states = {'Standby': 0, 'Push': 1, 'Scan': 2, 'Reposition': 3}
x = 0
y = 0
theta = 0
# TODO: Transition from Standby to a new goal state via subscribing
# Publish desired waypoints at the appropriate time.
def talker():
global waypoints, waypoint_number, path_complete, pub_waypoint
global pub_path_complete, pub_find_object, states, state
# Launch this node with the name "set_waypoints"
# let's define a function we will use later
def shutdownFunction():
print "we are done here."
# when the python interpreter runs this script
# this is the section that gets run as main
if __name__ == '__main__':
# let the master know about our name
rospy.init_node("riss")
# define a duration of 1 second
oneSecond = rospy.Duration(1, 0)
# define and fill out some messages
boolMsg = Bool()
boolMsg.data = 0
uint16Msg = UInt16()
uint16Msg.data = 10
uint16Pub = rospy.Publisher('unsigned_integer_topic', UInt16, queue_size=5)
boolPub = rospy.Publisher('bool_topic', Bool, queue_size=5)
# wait for Ctrl-C and loop until then
while not rospy.is_shutdown():
# print a message for feedback
print "looping..."
# publish messages
uint16Pub.publish(uint16Msg)
# Reset controllers, pause the sim, reset the world,
# unpause the sim and then release the controllers
winner_pub.publish(win_msg)
reset_msg.data = True
reset_pub.publish(reset_msg)
gazebo_reset()
reset_model(robot_1, robot_1_initial_pose)
reset_model(robot_2, robot_2_initial_pose)
reset_controllers()
rospy.sleep(rospy.Duration(0.1))
reset_msg.data = False
reset_pub.publish(reset_msg)
start_time = rospy.Time.now()
reset_pub = rospy.Publisher("/reset", Bool, queue_size=1)
reset_msg = Bool()
# Make sure all controllers are in the reset state
reset_msg.data = True
reset_pub.publish(reset_msg)
rospy.sleep(rospy.Duration(0.5))
robot_1_initial_pose = robot_pos(robot_1, "world").pose
robot_2_initial_pose = robot_pos(robot_2, "world").pose
sleeper = rospy.Rate(20)
robot_1_wins = 0
robot_2_wins = 0
draws = 0
it = 0
win_msg = String()
def action_start():
""" implements start end point """
if not check_action(request, START.url, START.methods):
return th_error()
log_das(LogError.INFO,
"%s hit with %s" % (START.url, request.get_json(silent=True)))
try:
j = request.get_json(silent=True)
params = TestAction(**j)
except Exception as e:
log_das(LogError.RUNTIME_ERROR,
'%s got a malformed test action POST: %s' % (START.url, e))
return th_error()
## check to see if there's already an assigned goal, abort if so.
global client
if client.gh:
log_das(LogError.RUNTIME_ERROR,
"%s hit with an already active goal" % START.url)
return th_error()
global deadline
global pub_user_notify
global desired_volts
global desired_bump
global pub_setvoltage
global pub_setcharging
if config.enable_adaptation == AdaptationLevels.CP2_Adaptation:
cw_log = open("/test/calibration_watcher.log", "w")
cw_child = subprocess.Popen([BINDIR + "/calibration_watcher"],
stdout=cw_log,
stderr=cw_log,
cwd="/home/vagrant/")
if in_cp2() and (not bump_sensor(desired_bump)):
log_das(LogError.RUNTIME_ERROR,
"Fatal: could not set inital sensor pose in %s" % START.url)
return th_error()
try:
pub_setcharging.publish(Bool(False))
pub_setvoltage.publish(Int32(desired_volts))
except Exception as e:
log_das(
LogError.RUNTIME_ERROR,
'%s got an error trying to publish to set_voltage and set_charging: %s'
% (START.url, e))
return th_error()
log_das(LogError.INFO, "starting challenge problem")
try:
with open(instruct('.ig')) as igfile:
igcode = igfile.read()
goal = ig_action_msgs.msg.InstructionGraphGoal(order=igcode)
client.send_goal(goal=goal, done_cb=done_cb, active_cb=active_cb)
# update the deadline to be now + the amount of time for the path
# given in the json file
with open(instruct('.json')) as config_file:
data = json.load(config_file)
deadline = int(data['time']) + rospy.Time.now().secs
pub_user_notify.publish(
UserNotification(new_deadline=str(deadline),
user_notification="initial deadline"))
except Exception as e:
log_das(LogError.RUNTIME_ERROR,
"could not send the goal in %s: %s " % (START.url, e))
return th_error()
res = action_result({})
log_das(LogError.INFO, "%s returning %s" % (START.url, resp2str(res)))
return res
#!/home/mhyde/vEnvs/rosPy/bin/python
import rospy
from std_msgs.msg import Bool
def pubReq():
while True:
usrInput = raw_input('Send True? :: [Y/n] :: ')
if usrInput.upper() == 'Y':
msg.data = True
pub.publish(msg)
rospy.loginfo(msg)
elif usrInput.lower() == 'end':
break
else:
msg.data = False
pub.publish(msg)
rospy.loginfo(msg)
msg = Bool()
pub = rospy.Publisher('arduinoState', Bool, queue_size=10)
rospy.init_node('dummyPub', anonymous=True)
pubReq()
def send_exe_camera(self, BOOL):
exe_msg = Bool()
exe_msg.data = BOOL
print("Publishing")
self.pub_camera.publish(exe_msg)
#!/usr/bin/env python
import sys
import rospy
import os
import sys
from gantry_control.msg import *
from return_control.srv import *
from std_msgs.msg import Bool
from std_srvs.srv import Empty, Trigger
from record_ros.srv import String_cmd
from data_manager.srv import *
import getFiles as files
trueMsg = Bool()
trueMsg.data = True
falseMsg = Bool()
falseMsg.data = False
camState = False
runComplete = False
SAVE_DATA = rospy.get_param('GLOBAL_SAVE_DATA')
DEBUG = False
def callbackCams(data):
global camState
camState = data.data
def main():
print "INITIALIZING TORSO NODE IN SIMULATION MODE BY MARCOSOFT..."
###Connection with ROS
global pubGoalReached
rospy.init_node("torso")
br = tf.TransformBroadcaster()
jointStates = JointState()
jointStates.name = [
"spine_connect", "waist_connect", "shoulders_connect",
"shoulders_left_connect", "shoulders_right_connect"
]
jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0]
rospy.Subscriber("/hardware/torso/goal_pose", Float32MultiArray,
callbackGoalPose)
rospy.Subscriber("/hardware/torso/goal_rel_pose", Float32MultiArray,
callbackRelPose)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubCurrentPose = rospy.Publisher("/hardware/torso/current_pose",
Float32MultiArray,
queue_size=1)
pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached",
Bool,
queue_size=1)
loop = rospy.Rate(30)
global goalSpine
global goalWaist
global goalShoulders
global spine
global waist
global shoulders
global newGoal
spine = 0.10
waist = 0
shoulders = 0
goalSpine = spine
goalWaist = waist
goalShoulders = shoulders
speedSpine = 0.005
speedWaist = 0.1
speedShoulders = 0.1
msgCurrentPose = Float32MultiArray()
msgGoalReached = Bool()
msgCurrentPose.data = [0, 0, 0]
newGoal = False
while not rospy.is_shutdown():
deltaSpine = goalSpine - spine
deltaWaist = goalWaist - waist
deltaShoulders = goalShoulders - shoulders
if deltaSpine > speedSpine:
deltaSpine = speedSpine
if deltaSpine < -speedSpine:
deltaSpine = -speedSpine
if deltaWaist > speedWaist:
deltaWaist = speedWaist
if deltaWaist < -speedWaist:
deltaWaist = -speedWaist
if deltaShoulders > speedShoulders:
deltaShoulders = speedShoulders
if deltaShoulders < -speedShoulders:
deltaShoulders = -speedShoulders
spine += deltaSpine
waist += deltaWaist
shoulders += deltaShoulders
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [
spine, waist, shoulders, -shoulders, -shoulders
]
pubJointStates.publish(jointStates)
msgCurrentPose.data[0] = spine
msgCurrentPose.data[1] = waist
msgCurrentPose.data[2] = shoulders
pubCurrentPose.publish(msgCurrentPose)
if newGoal and abs(goalSpine - spine) < 0.02 and abs(
goalWaist - waist) < 0.05 and abs(goalShoulders -
shoulders) < 0.05:
newGoal = False
msgGoalReached.data = True
pubGoalReached.publish(msgGoalReached)
loop.sleep()
j['linear_x'] = -NOW_SPEED
if hat[0] == 1:
j['angular_z'] = -0.3
if hat[0] == -1:
j['angular_z'] = 0.3
if joystick.get_button(2) == 1: # button B : shutdowwn
print("shutdown!!!!!!!!")
j = {
'linear_x': 0.0,
'linear_y': 0.0,
'linear_z': 0.0,
'angular_x': 0.0,
'angular_y': 0.0,
'angular_z': 0.0,
}
b = Bool()
b.data = True
pub.publish(b)
# only send msgs when the input is not none
if j['linear_x'] != 0.0 or j['angular_z'] != 0.0:
sendman.send(j)
print(j)
clock.tick(20)
# Close the window and quit.
# If you forget this line, the program will 'hang'
# on exit if running from IDLE.
pygame.quit()
def number_recognition_node():
global flag
global audioBuffer
sample = None
stop = False
global THRESHOLD_COEF
THRESHOLD_COEF = 0
rospy.init_node('number_recognition_node')
pub = rospy.Publisher('/audio/detected_numbers', String, queue_size=1)
threspub = rospy.Publisher('/audio/threshold_up', Bool, queue_size=1)
rospy.Subscriber('/audio/mic_samples', Int16MultiArray, audio_callback)
rospy.Subscriber('/audio/detection_flag', Bool, flag_callback)
rospy.Subscriber('/audio/threshold_val', Int16, threshold_callback)
#Block samples / second
rate = rospy.Rate(int((RATE / BLOCKSIZE)))
#Creates folder for image files
print(str(flag))
if not os.path.exists(sys.path[0] + '/tmp/tmp'):
os.makedirs(sys.path[0] + '/tmp/tmp/')
#Constantly publishes number detected
while not rospy.is_shutdown():
#If the audio recognition flag is up, starts recognition
if flag and audioBuffer != []:
#Published message
message = String()
#Starts defining threshold value with 20 samples
print('Defining volume threshold..')
prevAudioBuffer = []
while len(prevAudioBuffer) < 20 and not rospy.is_shutdown():
rate.sleep()
if audioBuffer != []:
prevAudioBuffer.append(audioBuffer.pop(0))
THRESHOLD, THRESHOLD_O = def_threshold(prevAudioBuffer)
print('Start talking')
#pub.publish(String('Start talking'))
# Wait until voice detected. Once detected, breaks the loop
# Also saves up to n prevSamples samples before threshold is reached
prevAudioBuffer = []
while True and not rospy.is_shutdown():
rate.sleep()
if audioBuffer != []:
sample = audioBuffer.pop(0)
prevAudioBuffer.append(sample)
if len(prevAudioBuffer) > MAX_SILENCE_START + 5:
prevAudioBuffer.pop(0)
threspub.publish(Bool(False))
#Test 5 last samples with threshold to detect input
if input_detected_start(prevAudioBuffer, THRESHOLD):
break
#print('Input detected')
threspub.publish(Bool(True))
if rospy.is_shutdown(): break
#Once an input is detected, threshold is adjusted down
THRESHOLD = THRESHOLD_O
#Output wave file is oppened
output_wf = wave.open(sys.path[0] + '/myNumber.wav', 'wb')
output_wf.setframerate(RATE)
output_wf.setsampwidth(WIDTH)
output_wf.setnchannels(CHANNELS)
#All samples are saved
while len(prevAudioBuffer) > 1:
output_wf.writeframes(save_sample(prevAudioBuffer.pop(0)))
#Starts recording until sound is under threshold for a fixed time
silence_count = 0
for n in range(0, int(LEN / BLOCKSIZE)):
#Takes current sample and saves it
output_wf.writeframes(save_sample(sample))
#Waiting for next sample
while audioBuffer == [] and not rospy.is_shutdown():
pass
#Takes new sample
sample = audioBuffer.pop(0)
#Verifies volume of new sample for MAX_SILENCE_END samples up to LEN seconds
if not input_detected_end(sample, THRESHOLD):
silence_count += 1
if silence_count == MAX_SILENCE_END:
print('Input stopped')
break
else:
silence_count = 0
threspub.publish(Bool(False))
#Closes WAV file
output_wf.close()
#Cleans audio buffer
audioBuffer = []
#CNN prediction
try:
to_png(sys.path[0] + '/myNumber.wav',
sys.path[0] + '/tmp/tmp/myNumber.png')
message.data = str(predict(sys.path[0] + '/myNumber.png'))
print('Predicted as: %s' % (message.data))
if message.data != "None":
message.data = "Predicted as: " + message.data
pub.publish(message)
except:
print('Short sample')
#print("wait")
rate.sleep()
def SetPublishLogger(self, allowPublishLogging):
self.logger.SetPublishLogger(self._to_cpp(Bool(allowPublishLogging)))
def stopSim(self):
self.stopSimPub.publish(Bool(data=True))
def callback(data): button = Bool() button.data = data.buttons[1] pub.publish(button)
def synchronousSimulation(self, mode):
_bool = Bool(data=mode)
self.enableSyncModePub.publish(_bool)
self._counter = 0
lightPubs = {}
addressMap = {}
for robot in robotNames:
key = str((len(robotKeyNameMap) + 1) % 10)
robotKeyNameMap[key] = robot
velPubs[key] = rospy.Publisher(robot + '/cmd_vel_relay',
Twist,
queue_size=1)
commPubs[key] = rospy.Publisher(robot + '/comm', String, queue_size=1)
selPubs[key] = rospy.Publisher(robot + '/select', Bool, queue_size=1)
lightPubs[key] = rospy.Publisher(robot + '/light', Bool, queue_size=1)
addressMap[key] = robotAddressMap[robot]
if len(robotKeyNameMap) == 10:
break
currentRobotKey = '1'
flag = Bool()
flag.data = True
selPubs[currentRobotKey].publish(flag)
speed = rospy.get_param("~speed", 0.5)
turn = rospy.get_param("~turn", 1.0)
x = 0
y = 0
z = 0
th = 0
status = 0
try:
print(msg)
print('--------------------------')
print('Robot List:')
def main(portName1, simulated):
print "INITIALIZING TORSO..."
###Connection with ROS
rospy.init_node("torso")
jointStates = JointState()
jointStates.name = [
"spine_connect", "waist_connect", "shoulders_connect",
"shoulders_left_connect", "shoulders_right_connect"
]
jointStates.position = [0.0, 0.0, 0.0, 0.0, 0.0]
pubTorsoPos = rospy.Publisher("/hardware/torso/current_pose",
Float32MultiArray,
queue_size=1)
pubGoalReached = rospy.Publisher("/hardware/torso/goal_reached",
Bool,
queue_size=1)
pubJointStates = rospy.Publisher("/joint_states", JointState, queue_size=1)
pubStop = rospy.Publisher("robot_state/stop", Empty, queue_size=1)
pubEmergencyStop = rospy.Publisher("robot_state/emergency_stop",
Bool,
queue_size=1)
subRelativeHeight = rospy.Subscriber("/hardware/torso/goal_rel_pose",
Float32MultiArray, callbackRelative)
subAbsoluteHeight = rospy.Subscriber("/hardware/torso/goal_pose",
Float32MultiArray, callbackAbsolute)
subStop = rospy.Subscriber("robot_state/stop", Empty, callbackStop)
subEmergencyStop = rospy.Subscriber("robot_state/emergency_stop", Bool,
callbackEmergency)
subTorsoUp = rospy.Subscriber("/hardware/torso/torso_up", String,
callbackTorsoUp)
subTorsoDown = rospy.Subscriber("/hardware/torso/torso_down", String,
callbackTorsoDown)
rate = rospy.Rate(ITER_RATE)
global valueRel
global valueAbs
global absH
global relH
global stop
global torsoUp
global torsoDown
global eme_stop
global new_eme_msg_recv
valueAbs = False
valueRel = False
torsoUp = False
torsoDown = False
torsoPos = 0
bumper = 0
stop = False
msgCurrentPose = Float32MultiArray()
msgGoalReached = Bool()
msgCurrentPose.data = [0, 0, 0]
msgMotor = None
initTimeMtrMsg = datetime.now()
initTimeSnrMsg = datetime.now()
timeoutSnr = 0
timeoutMtr = 0
ArdIfc = comm.Comm(portName1)
msgSensor = comm.Msg(comm.ARDUINO_ID, comm.MOD_SENSORS,
comm.OP_GETCURRENTDIST, [], 0)
ArdIfc.send(msgSensor)
goalPose = 0
new_eme_msg_recv = False
eme_stop = Bool()
eme_stop.data = False
while not rospy.is_shutdown():
try:
timeoutSnr = datetime.now() - initTimeSnrMsg
if timeoutSnr.microseconds > MSG_SENSOR_TIMEOUT:
ArdIfc.send(msgSensor)
initTimeSnrMsg = datetime.now()
newMsg = ArdIfc.recv()
if newMsg != None:
if newMsg.mod == comm.MOD_SENSORS:
if newMsg.op == comm.OP_GETCURRENTDIST:
torsoPos = newMsg.param[0]
#rospy.loginfo("Torso-> Arduino ack GET CURRENT DIST msg received.")
if newMsg.mod == comm.MOD_SYSTEM:
if newMsg.op == comm.OP_PING:
rospy.loginfo("Torso-> Arduino ack PING msg received.")
if newMsg.op == comm.OP_STOP:
rospy.loginfo(
"Torso-> Arduino Emercenty STOP system received. "
)
if eme_stop.data != bool(newMsg.param[0]):
eme_stop.data = newMsg.param[0]
pubEmergencyStop.publish(eme_stop)
if newMsg.mod == comm.MOD_MOTORS:
if newMsg.op == comm.OP_SETTORSOPOSE:
msgMotor_ack_received = True
rospy.loginfo(
"Torso-> Arduino ack SETTORSOPOSE msg received.")
if newMsg.op == comm.OP_GOUP:
msgMotor_ack_received = True
rospy.loginfo("Torso-> Arduino ack GOUP msg received.")
if newMsg.op == comm.OP_GODOWN:
msgMotor_ack_received = True
rospy.loginfo(
"Torso-> Arduino ack GODOWN msg received.")
if newMsg.op == comm.OP_STOP_MOTOR:
msgMotor_ack_received = True
#rospy.loginfo("Torso-> Arduino ack STOP MOTOR msg received.")
#until ack received
timeoutMtr = datetime.now() - initTimeMtrMsg
if msgMotor != None and timeoutMtr.microseconds > MSG_MOTOR_TIMEOUT and not msgMotor_ack_received:
ArdIfc.send(msgMotor)
initTimeMtrMsg = datetime.now()
if valueAbs and not eme_stop.data and absH >= DIST_LIM_INF and absH <= DIST_LIM_SUP:
msgMotor_ack_received = False
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_SETTORSOPOSE, int(absH), 1)
ArdIfc.send(msgMotor)
valueAbs = False
goalPose = absH
initTimeMtrMsg = datetime.now()
elif valueRel and not eme_stop.data and (
torsoPos + relH) >= DIST_LIM_INF and (
torsoPos + relH) <= DIST_LIM_SUP:
msgMotor_ack_received = False
absCalH = torsoPos + relH
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_SETTORSOPOSE, int(absCalH), 1)
ArdIfc.send(msgMotor)
goalPose = absCalH
valueRel = False
initTimeMtrMsg = datetime.now()
elif (valueAbs and
(absH < DIST_LIM_INF or absH > DIST_LIM_SUP)) or (
valueRel and (torsoPos + relH > DIST_LIM_SUP
or torsoPos + relH < DIST_LIM_INF)):
rospy.logerr("Torso-> Can not reach te position.")
valueAbs = False
valueRel = False
goalPose = torsoPos
elif torsoUp and not eme_stop.data:
rospy.loginfo("Torso-> Moving torso up.")
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_GOUP, [], 0)
ArdIfc.send(msgMotor)
torsoUp = False
msgMotor_ack_received = False
initTimeMtrMsg = datetime.now()
goalPose = torsoPos
elif torsoDown and not eme_stop.data:
rospy.loginfo("Torso-> Moving torso down.")
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_GODOWN, [], 0)
ArdIfc.send(msgMotor)
torsoDown = False
msgMotor_ack_received = False
initTimeMtrMsg = datetime.now()
goalPose = torsoPos
elif eme_stop.data and new_eme_msg_recv:
rospy.loginfo("Torso-> Stop message.")
msgMotor = comm.Msg(comm.ARDUINO_ID, comm.MOD_MOTORS,
comm.OP_STOP_MOTOR, [], 0)
ArdIfc.send(msgMotor)
msgMotor_ack_received = False
initTimeMtrMsg = datetime.now()
new_eme_msg_recv = False
torsoDown = False
torsoUp = False
valueAbs = False
valueRel = False
jointStates.header.stamp = rospy.Time.now()
jointStates.position = [(torsoPos - TORSO_ADJUSTMENT) / 100.0, 0.0,
0.0, 0.0, 0.0]
pubJointStates.publish(jointStates)
msgCurrentPose.data[0] = (torsoPos - TORSO_ADJUSTMENT) / 100.0
msgCurrentPose.data[1] = 0.0
msgCurrentPose.data[2] = 0.0
pubTorsoPos.publish(msgCurrentPose)
msgGoalReached.data = abs(goalPose - torsoPos) < THR_DIFF_POS
pubGoalReached.publish(msgGoalReached)
rate.sleep()
except:
rospy.logerr(
"Torso-> Oopps...we have some issues in this node :( ")
def __init__(self):
rospy.Subscriber("rob_id", rob_id, self.publisher_id)
self.id_pub_1 = rospy.Publisher(sys.argv[1] + "/robot_id", Bool, queue_size=1)
self.id_pub_2 = rospy.Publisher(sys.argv[2] + "/robot_id", Bool, queue_size=1)
self._robot_id = Bool()
self._robot_id.data = True
def reset_joint_positions(self):
msg = Bool()
msg.data = True
self.publisher.publish(msg)
rospy.loginfo(
"Hardware Joint Resetter: Wheel positions reset to zero.")
def requestNewWaypoint(self):
req = Bool()
req.data = False # Random frontier flag to false
self.pub_replan.publish(req)
def find_object():
global pub_find_object
pub_find_object.publish(Bool(True))
def publish_state(self):
bmsg = Bool()
bmsg.data = self.recording
self.rec_pub.publish(bmsg)
def publish_dbw_status(self, data):
self.publishers['dbw_status'].publish(Bool(data))
#========================#
PI = 3.141592653589793
ardrone = States()
states = States()
states.x = 0.0
states.y = 0.0
states.u = 0.0
states.v = 0.0
g = 9.81
L = 1
pen_in = PointStamped()
rate = 200 # Hz
pub_states = rospy.Publisher('/cortex_raw', Cortex, queue_size=1)
start_time = 0
controller_status = Bool()
controller_status.data = False
#==================#
# Get States #
#==================#
def GetStates(I):
global ardrone
ardrone = I
#=============================#
# Get Controller Status #
#=============================#
stop_wait = time.time() + wait_open
path = []
while not rospy.is_shutdown():
r = rospy.Rate(1) # 3hz
r.sleep()
# If lidar detects right door, go in. Else after some time, go left.
if elevator == "right":
rospy.set_param('/elevator', 'right')
inside1 = Twist()
inside1.linear.x, inside1.linear.y = 53.4, 3.9
inside2 = Twist()
inside2.linear.x, inside2.linear.y, inside2.angular.z = 53.4, 3.9, 3.14
path = [inside1, inside2]
break
elif time.time() >= stop_wait:
# left if right didn't open
rospy.set_param('/elevator', 'left')
outside = Twist()
outside.linear.x, outside.linear.y = 51.5, 6.8
inside1 = Twist()
inside1.linear.x, inside1.linear.y = 53.4, 6.8
inside2 = Twist()
inside2.linear.x, inside2.linear.y, inside2.angular.z = 53.4, 7.0, 3.14
path = [outside, inside1, inside2]
break
wp_pub = WaypointPublisher(path)
while not wp_pub.done and not rospy.is_shutdown():
r = rospy.Rate(10)
r.sleep()
done_pub.publish(Bool(data=True))
def update(self):
self.state_lock.acquire()
state = copy.copy(self.state_obj)
self.state_lock.release()
if state is None:
return
self.program_runtime_state = state.runtime_state
self.parse_and_publish_state(state)
if self.program_runtime_state != hp.UR_RUNNING_FLAG:
self.robot_ready = False
self.pub_robot_ready.publish(Bool(False))
self.ping_id = []
self.ros_rate.sleep()
return
if not self.robot_ready and self.ping_id == []:
self.ping_id = int(state.output_int_register_1 != 1)
hp.list_to_input(self.ping_obj, self.ping_id, 1,
hp.PING_INT_REGISTER, "int")
self.con.send(self.ping_obj)
elif not self.robot_ready and self.ping_id == state.output_int_register_1:
self.robot_ready = True
self.pub_robot_ready.publish(Bool(True))
print "ROBOT READY"
self.command_lock.acquire()
mode = self.mode
setp = self.setp
avtr = self.avtr
setp2 = self.setp2
enable = self.enable
self.command_lock.release()
if self.mode_prev != mode:
self.start_new_mode(mode)
self.mode_prev = mode
# do the update for each mode
if mode == hp.STANDBY_MODE: # STANDBY MODE
hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER,
"int")
hp.list_to_input(self.enable_obj, enable, 1,
hp.ENABLE_INT_REGISTER, "int")
self.con.send(self.mode_obj)
self.con.send(self.enable_obj)
elif mode == hp.FREEDRIVE_MODE: # FREEDRIVE MODE
hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER,
"int")
hp.list_to_input(self.enable_obj, enable, 1,
hp.ENABLE_INT_REGISTER, "int")
self.con.send(self.mode_obj)
self.con.send(self.enable_obj)
elif mode == hp.WAYPOINT_JOINT_MODE: # WAYPOINT JOINT MODE
hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER,
"int")
hp.list_to_input(self.setp_obj, setp, 6,
hp.SETP_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.avtr_obj, avtr, 4,
hp.AVTR_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.enable_obj, enable, 1,
hp.ENABLE_INT_REGISTER, "int")
# send new setpoint
self.con.send(self.mode_obj)
self.con.send(self.setp_obj)
self.con.send(self.avtr_obj)
self.con.send(self.enable_obj)
elif mode == hp.POSITION_CONTROL_JOINT_MODE: # JOINT POSITION CONTROL MODE
hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER,
"int")
hp.list_to_input(self.setp_obj, setp, 6,
hp.SETP_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.servoj_params_obj, self.servoj_params, 3,
hp.SERVOJ_PARAMS_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.enable_obj, enable, 1,
hp.ENABLE_INT_REGISTER, "int")
# send new setpoint
self.con.send(self.mode_obj)
self.con.send(self.setp_obj)
self.con.send(self.servoj_params_obj)
self.con.send(self.enable_obj)
elif mode == hp.VELOCITY_CONTROL_JOINT_MODE: # JOINT SPEED CONTROL MODE
hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER,
"int")
hp.list_to_input(self.setp_obj, setp, 6,
hp.SETP_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.avtr_obj, avtr, 4,
hp.AVTR_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.enable_obj, enable, 1,
hp.ENABLE_INT_REGISTER, "int")
# send new setpoint
self.con.send(self.mode_obj)
self.con.send(self.setp_obj)
self.con.send(self.avtr_obj)
self.con.send(self.enable_obj)
elif mode == hp.POSITION_CONTROL_CARTESIAN_MODE: # CARTESIAN POSITION CONTROL MODE
hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER,
"int")
hp.list_to_input(self.setp_obj, setp, 6,
hp.SETP_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.servoj_params_obj, self.servoj_params, 3,
hp.SERVOJ_PARAMS_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.enable_obj, enable, 1,
hp.ENABLE_INT_REGISTER, "int")
# send new setpoint
self.con.send(self.mode_obj)
self.con.send(self.setp_obj)
self.con.send(self.servoj_params_obj)
self.con.send(self.enable_obj)
elif mode == hp.IMPEDANCE_CONTROL_MODE: # IMPEDANCE CONTROL MODE
hp.list_to_input(self.mode_obj, mode, 1, hp.MODE_INT_REGISTER,
"int")
hp.list_to_input(self.setp_obj, setp, 6,
hp.SETP_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.setp2_obj, setp2, 6,
hp.SETP2_DOUBLE_REGISTER_OFFSET, "double")
hp.list_to_input(self.enable_obj, enable, 1,
hp.ENABLE_INT_REGISTER, "int")
# send new setpoint
self.con.send(self.mode_obj)
self.con.send(self.setp_obj)
self.con.send(self.setp2_obj)
self.con.send(self.enable_obj)
self.ros_rate.sleep()
def main():
# NOR gate RS flip flop example
# Input topics /S and /R are active high (true is logic 1 and false is logic 0)
# Output topics /Q and /Q_not are active low (true is logic 0 and false is logic 1)
# Input/Output table
# S: false R: false | Q: no change Q_not: no change
# S: true R: false | Q: false Q_not: true
# S: false R: true | Q: true Q_not: false
# S: true R: true | Q: invalid Q_not: invalid
builder = DiagramBuilder()
sys_ros_interface = builder.AddSystem(RosInterfaceSystem())
qos = QoSProfile(depth=10)
sys_pub_Q = builder.AddSystem(
RosPublisherSystem(Bool, "Q", qos,
sys_ros_interface.get_ros_interface()))
sys_pub_Q_not = builder.AddSystem(
RosPublisherSystem(Bool, "Q_not", qos,
sys_ros_interface.get_ros_interface()))
sys_sub_S = builder.AddSystem(
RosSubscriberSystem(Bool, "S", qos,
sys_ros_interface.get_ros_interface()))
sys_sub_R = builder.AddSystem(
RosSubscriberSystem(Bool, "R", qos,
sys_ros_interface.get_ros_interface()))
sys_nor_gate_1 = builder.AddSystem(NorGate())
sys_nor_gate_2 = builder.AddSystem(NorGate())
sys_memory = builder.AddSystem(Memory(Bool()))
builder.Connect(sys_nor_gate_1.GetOutputPort('Q'),
sys_memory.get_input_port(0))
builder.Connect(
sys_sub_S.get_output_port(0),
sys_nor_gate_1.GetInputPort('A'),
)
builder.Connect(
sys_nor_gate_2.GetOutputPort('Q'),
sys_nor_gate_1.GetInputPort('B'),
)
builder.Connect(
sys_memory.get_output_port(0),
sys_nor_gate_2.GetInputPort('A'),
)
builder.Connect(
sys_sub_R.get_output_port(0),
sys_nor_gate_2.GetInputPort('B'),
)
builder.Connect(sys_nor_gate_1.GetOutputPort('Q'),
sys_pub_Q.get_input_port(0))
builder.Connect(sys_nor_gate_2.GetOutputPort('Q'),
sys_pub_Q_not.get_input_port(0))
diagram = builder.Build()
simulator = Simulator(diagram)
simulator_context = simulator.get_mutable_context()
simulator.set_target_realtime_rate(1.0)
while True:
simulator.AdvanceTo(simulator_context.get_time() + 0.1)
def __init__(self, quad_name, environment="gazebo", recording_options=None, load_options=None, use_ekf=False,
rdrv=None, plot=False, reset_experiment=False):
if recording_options is None:
recording_options = {"recording": False}
# If on a simulation environment, figure out if physics are slowed down
if environment == "gazebo":
try:
get_gazebo_physics = rospy.ServiceProxy('/gazebo/get_physics_properties', GetPhysicsProperties)
resp = get_gazebo_physics()
physics_speed = resp.max_update_rate * resp.time_step
rospy.loginfo("Physics running at %.2f normal speed" % physics_speed)
except rospy.ServiceException as e:
print("Service call failed: %s" % e)
physics_speed = 1
else:
physics_speed = 1
self.physics_speed = physics_speed
self.environment = environment
self.plot = plot
self.recording_options = recording_options
# Control at 50 (sim) or 60 (real) hz. Use time horizon=1 and 10 nodes
self.n_mpc_nodes = rospy.get_param('~n_nodes', default=10)
self.t_horizon = rospy.get_param('~t_horizon', default=1.0)
self.control_freq_factor = rospy.get_param('~control_freq_factor', default=5 if environment == "gazebo" else 6)
self.opt_dt = self.t_horizon / (self.n_mpc_nodes * self.control_freq_factor)
# Load trained GP model
if load_options is not None:
rospy.loginfo("Attempting to load GP model from:\n git: {}\n name: {}\n meta: {}".format(
load_options["git"], load_options["model_name"], load_options["params"]))
pre_trained_models = load_pickled_models(model_options=load_options)
if pre_trained_models is None:
rospy.logwarn("Model parameters specified did not match with any pre-trained GP")
else:
pre_trained_models = None
self.pre_trained_models = pre_trained_models
self.git_v = load_options["git"]
if self.pre_trained_models is not None:
rospy.loginfo("Successfully loaded GP model")
self.model_name = load_options["model_name"]
elif rdrv is not None:
self.model_name = "rdrv"
else:
self.model_name = "Nominal"
# Initialize GP MPC for point tracking
self.gp_mpc = ROSGPMPC(self.t_horizon, self.n_mpc_nodes, self.opt_dt, quad_name=quad_name,
point_reference=False, gp_models=pre_trained_models, rdrv=rdrv)
# Last state obtained from odometry
self.x = None
# Elapsed time between two recordings
self.last_update_time = time.time()
# Last references. Use hovering activation as input reference
self.last_x_ref = None
self.last_u_ref = None
# Reference trajectory variables
self.x_ref = None
self.t_ref = None
self.u_ref = None
self.current_idx = 0
self.quad_trajectory = None
self.quad_controls = None
self.w_control = None
# Provisional reference for "waiting_for_reference" hovering mode
self.x_ref_prov = None
# To measure optimization elapsed time
self.optimization_dt = 0
# Thread for MPC optimization
self.mpc_thread = threading.Thread()
# Trajectory tracking experiment. Dims: seed x av_v x n_samples
if reset_experiment:
self.metadata_dict = {}
else:
self.metadata_dict, _, _, _ = load_past_experiments()
self.mse_exp = np.zeros((0, 0, 0, 1))
self.t_opt = np.zeros((0, 0, 0))
self.mse_exp_v_max = np.zeros((0, 0))
self.ref_traj_name = ""
self.ref_v = 0
self.run_traj_counter = 0
# Keep track of status of MPC object
self.odom_available = False
# Binary variable to run MPC only once every other odometry callback
self.optimize_next = True
# Binary variable to completely skip an odometry if in flying arena
self.skip_next = False
# Remember the sequence number of the last odometry message received.
self.last_odom_seq_number = 0
# Measure if trajectory starting point is reached
self.x_initial_reached = False
# Variables for recording mode
self.recording_warmup = True
self.x_pred = None
self.w_opt = None
# Odometry estimate for GP. None by default if same odometry as control reference should be used
self.gp_odom = None
# Get recording file and directory
blank_recording_dict = make_record_dict(state_dim=13)
if recording_options["recording"]:
record_raw_optitrack = recording_options["record_raw_optitrack"]
overwrite = recording_options["overwrite"]
metadata = {self.environment: "default"}
rec_dict, rec_file = get_record_file_and_dir(
blank_recording_dict, recording_options, simulation_setup=metadata, overwrite=overwrite)
# If flying with the optitrack system, also record raw optitrack estimates
if self.environment == "flying_room" or self.environment == 'arena' and record_raw_optitrack:
rec_dict_raw = make_raw_optitrack_dict()
metadata = {self.environment: "optitrack_raw"}
rec_dict_raw, rec_file_raw = get_record_file_and_dir(
rec_dict_raw, recording_options, simulation_setup=metadata, overwrite=overwrite)
else:
rec_dict_raw = rec_file_raw = None
else:
record_raw_optitrack = False
rec_dict = rec_file = None
rec_dict_raw = rec_file_raw = None
self.rec_dict = rec_dict
self.rec_file = rec_file
self.rec_dict_raw = rec_dict_raw
self.rec_file_raw = rec_file_raw
self.landing = False
self.override_land = False
self.ground_level = False
self.controller_off = False
# Setup node publishers and subscribers. The odometry (sub) and control (pub) topics will vary depending on
# which environment is being used
ekf_odom_topic = None
if self.environment == "gazebo":
odom_topic = "/" + quad_name + "/ground_truth/odometry"
raw_topic = None
elif self.environment == "arena":
# Assume arena world setup
odom_topic = "/" + quad_name + "/state_estimate"
raw_topic = "/vicon/" + quad_name
if use_ekf:
ekf_odom_topic = "/" + quad_name + "/state_estimate_ekf"
else:
# Assume real world setup
odom_topic = "/" + quad_name + "/state_estimate"
raw_topic = "/optitrack/" + quad_name
if use_ekf:
ekf_odom_topic = "/" + quad_name + "/state_estimate_ekf"
land_topic = "/" + quad_name + "/autopilot/land"
control_topic = "/" + quad_name + "/autopilot/control_command_input"
off_topic = "/" + quad_name + "/autopilot/off"
reference_topic = "reference"
status_topic = "busy"
# Publishers
self.control_pub = rospy.Publisher(control_topic, ControlCommand, queue_size=1, tcp_nodelay=True)
self.status_pub = rospy.Publisher(status_topic, Bool, queue_size=1)
self.off_pub = rospy.Publisher(off_topic, Empty, queue_size=1)
# Subscribers
self.land_sub = rospy.Subscriber(land_topic, Empty, self.land_callback)
self.ref_sub = rospy.Subscriber(reference_topic, ReferenceTrajectory, self.reference_callback)
if ekf_odom_topic:
# We get a second odometry estimate which is smoothed. Used to evaluate the GP's.
self.odom_sub = rospy.Subscriber(
odom_topic, Odometry, self.odometry_callback, queue_size=1, tcp_nodelay=True)
self.ekf_odom_sub = rospy.Subscriber(
ekf_odom_topic, Odometry, self.ekf_odom_callback, queue_size=1, tcp_nodelay=True)
else:
self.odom_sub = rospy.Subscriber(
odom_topic, Odometry, self.odometry_callback, queue_size=1, tcp_nodelay=True)
if raw_topic is not None and record_raw_optitrack:
self.raw_sub = rospy.Subscriber(raw_topic, PoseStamped, self.raw_odometry_callback)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
# Publish if MPC is busy with a current trajectory
msg = Bool()
msg.data = not (self.x_ref is None and self.odom_available)
self.status_pub.publish(msg)
rate.sleep()
