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()
global ff_pwm
ff_pwm = 0 # int
min_motor_position = 990
max_motor_position = 3525
max_motor_velocity = 1023 # signed 10-bit int with units 0.229 rpm/bit
if motor_controller is ControlApproach.POSITION or motor_controller is ControlApproach.CASCADE:
bag = rosbag.Bag('actual_position.bag', 'w')
elif motor_controller is ControlApproach.VELOCITY:
bag = rosbag.Bag('actual_velocity.bag', 'w')
global bag_data
bag_pos = Int32()
bag_error = Int32()
bag_pwm = Int32()
def pen_down():
global pen_pos_up
print('\nPEN DOWN...\n')
pen_pos_up = False
stop_motors()
time.sleep(.1)
pen_down_msg = SetMotorPosition()
pen_down_msg.id = 2
pen_down_msg.position = 1275
#! /usr/bin/env python
import rospy
from std_msgs.msg import Int32
rospy.init_node('topic_publisher')
pub = rospy.Publisher('/counter', Int32, queue_size=1)
#create a publsher object that publish on the /counter topic
rate = rospy.Rate(2)
count = Int32() #create a var of type Int32
count.data = 0
while not rospy.is_shutdown(): #until someone stops
#the program execution
pub.publish(count)
count.data += 1
rate.sleep()
def pr2_mover(object_list):
'''***********************************************************************
# TODO: Initialize variables
***********************************************************************'''
escena = 3
yaml_filename_output = "output_0.yaml"
test_scene_num = Int32()
test_scene_num.data = escena
dict_list = []
'''***********************************************************************
# TODO: Get/Read parameters
***********************************************************************'''
object_list_param = rospy.get_param('/object_list')
dropbox_list_param = rospy.get_param('/dropbox')
'''***********************************************************************
# TODO: Parse parameters into individual variables
***********************************************************************'''
for object in object_list:
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
#Obtaining name of the object
object_name.data = str(object.label)
#Obtaining group of the object
for item in object_list_param:
if item['name'] == object_name.data :
object_group = item['group']
# TODO: Rotate PR2 in place to capture side tables for the collision map
'''***********************************************************************
# TODO: Get the PointCloud for a given object and obtain it's centroid
***********************************************************************'''
points_arr = ros_to_pcl(object.cloud).to_array()
cloud = np.mean(points_arr, axis=0)[:3]
pick_pose.position.x = np.asscalar(cloud[0])
pick_pose.position.y = np.asscalar(cloud[1])
pick_pose.position.z = np.asscalar(cloud[2])
'''***********************************************************************
# TODO: Create 'place_pose' for the object
***********************************************************************'''
for item in dropbox_list_param:
if item['group'] == object_group:
position = item['position']
place_pose.position.x = position[0]
place_pose.position.y = position[1]
place_pose.position.z = position[2]
'''***********************************************************************
# TODO: Assign the arm to be used for pick_place
***********************************************************************'''
if object_group == 'red':
arm_name.data = str('left')
else:
arm_name.data = str('right')
'''************************************************************************************************
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
************************************************************************************************'''
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
'''***********************************************************************
# TODO: Insert your message variables to be sent as a service request
***********************************************************************'''
resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose)
print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
def __init__(self):
# Initialize the UAV
self.id = input("Drone ID #: ")
self.uav_id = "uav" + str(self.id)
rospy.init_node("controller" + str(self.id), anonymous=True)
self.rate = rospy.Rate(
100
) # Determine what this value should be to maximize performance in sim and real world scenarios
# Define Collision Cylinders
# Change these later to a dynamic value that depends on breaking distance so Rc = Rc +Dbr 'breaking distance'.
self.reserved_cyl_radius = 2
self.reserved_cyl_vertical = 1
self.blocking_cyl_radius = self.reserved_cyl_radius
self.blocking_cyl_vertical = 2
self.conflict_state = {'xy': 'xy-free', 'z': 'z-free'}
self.escape_angle = 0
# Setup the cylindrical obstacle diagram (COD)
self.COD = {'distance': {}, 'angle': {}, 'z-diff': {}}
# Create required publsihers for control
self.target_pos_pub = rospy.Publisher(
self.uav_id + "/mavros/setpoint_position/local",
PoseStamped,
queue_size=1)
self.target_vel_pub = rospy.Publisher(
self.uav_id + "/mavros/setpoint_velocity/cmd_vel",
TwistStamped,
queue_size=1)
# Create required subcribers for information and control
self.state_sub = rospy.Subscriber(self.uav_id + "/mavros/state", State,
self.state_changed_callback)
self.actual_pos_sub = rospy.Subscriber(
self.uav_id + "/mavros/local_position/pose", PoseStamped,
self.position_callback)
self.actual_pos_sub = rospy.Subscriber(
self.uav_id + "/mavros/global_position/global", NavSatFix,
self.global_pos_changed_callback)
self.actual_vel_sub = rospy.Subscriber(
self.uav_id + "/mavros/local_position/velocity_local",
TwistStamped, self.velocity_callback)
self.heading_sub = rospy.Subscriber(
self.uav_id + "/mavros/global_position/compass_hdg", Float64,
self.heading_changed_callback)
self.battery_voltage_sub = rospy.Subscriber(
self.uav_id + "/mavros/battery", BatteryState,
self.battery_voltage_callback)
self.network_state_sub = rospy.Subscriber(
"/network_map", Int32, self.network_state_changed_callback)
# Create Objects to pair with above pubs and subs
self.desired_velocity_object = TwistStamped()
self.battery_voltage_object = BatteryState()
self.actual_global_pos_object = NavSatFix()
self.neighbor_global_position_objects = {}
self.desired_pos_object = PoseStamped()
self.actual_pos_object = PoseStamped()
self.actual_velocity_object = Twist()
self.neighbor_velocity_objects = {}
self.neighbor_position_objects = {}
self.neighbor_global_pos_sub = {}
self.num_nodes_in_swarm = Int32()
self.mode_object = SetMode()
self.neighbor_pos_sub = {}
self.neighbor_vel_sub = {}
self.heading = Float64()
self.state = State()
self.currently_avoiding = False
self.count = 0
self.set_failsafe_action(2)
self.set_param('MPC_ACC_HOR_MAX', 1.0)
def pr2_mover(object_list):
# TODO: Initialize variables
test_scene_num = Int32()
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
dict_list = []
labels = []
centroids = [] # to be list of tuples (x, y, z)
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_list_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
object_list_group = []
object_list_name = []
dropbox_group = []
for item in object_list_param:
object_list_name.append(item['name'])
object_list_group.append(item['group'])
for item in dropbox_list_param:
dropbox_group.append(item['group'])
test_scene_num.data = 1
# TODO: Loop through the pick list
for object in object_list:
#Fiding the object group and name
object_group = object_list_param[object_list_name.index(
object.label)]['group']
object_name.data = str(object.label)
#Fiding place_pose and arm name
arm_name.data = str(
dropbox_list_param[dropbox_group.index(object_group)]['name'])
pose = (
dropbox_list_param[dropbox_group.index(object_group)]['position'])
place_pose.position = Point(pose[0], pose[1], pose[2])
# Pick_pose
points_arr = ros_to_pcl(object.cloud).to_array()
centroid = (np.mean(points_arr, axis=0)[:3])
#asscalar solved TypeError: data type not understood
pick_pose.position = Point(np.asscalar(centroid[0]),
np.asscalar(centroid[1]),
np.asscalar(centroid[2]))
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
yaml_filename = 'output_' + str(test_scene_num.data) + '.yaml'
send_to_yaml(yaml_filename, dict_list)
'''
def pr2_mover(object_list):
# TODO: init list to save objs
dict_list = []
centroids = [] # to be list of tuples (x, y, z)
# TODO: get list of objs form parameter server
objects_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: create the dictionary of dropbox
dropbox = {}
for p in dropbox_param:
dropbox[p['name']] = p['position']
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for obj in objects_param:
# TODO: get the name of the obj
object_name = String()
object_name.data = obj['name']
true_label = object_name.data
#set default value of pick_pose
pick_pose = Pose()
pick_pose.position.x = 0
pick_pose.position.y = 0
pick_pose.position.z = 0
#set orientation to 0
pick_pose.orientation.x = 0
pick_pose.orientation.y = 0
pick_pose.orientation.z = 0
pick_pose.orientation.w = 0
#set place pose orientation to 0
place_pose = Pose()
place_pose.orientation.x = 0
place_pose.orientation.y = 0
place_pose.orientation.z = 0
place_pose.orientation.w = 0
for detected_object in object_list:
pridicted_label = detected_object.label
if pridicted_label == true_label:
# TODO: Create 'place_pose' for the object
points_arr = ros_to_pcl(detected_object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
pick_pose_np = np.mean(points_arr, axis=0)[:3]
pick_pose.position.x = np.asscalar(pick_pose_np[0])
pick_pose.position.y = np.asscalar(pick_pose_np[1])
pick_pose.position.z = np.asscalar(pick_pose_np[2])
print("------")
print("centroid is :")
print(np.mean(points_arr, axis=0)[:3])
break
# TODO: Assign the arm to be used for pick_place
arm_name = String()
if obj['group'] == 'red':
arm_name.data = 'left'
elif obj['group'] == 'green':
arm_name.data = 'right'
else:
print "ERROR, group must be green or red!"
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
test_scene_num = Int32()
test_scene_num.data = 3
place_pose.position.x = dropbox[arm_name.data][0]
place_pose.position.y = dropbox[arm_name.data][1]
place_pose.position.z = dropbox[arm_name.data][2]
dict_list.append(
make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose,
place_pose))
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
# TODO: Output your request parameters into output yaml file
yaml_filename = "output_" + str(test_scene_num.data) + ".yaml"
send_to_yaml(yaml_filename, dict_list)
def tilt_up(self, deg):
pa = rospy.get_param("/robot/get_tilt")
pn = pa + deg
v = Int32(pn)
self.blackboard["pub_tilt"].publish(v)
def pr2_mover(object_list, object_table):
print("Starting Pick and Place")
# set the test scene number
TEST_SCENE_NUM = Int32()
TEST_SCENE_NUM.data = 1
# get the object centroids
object_labels, object_centroids = find_centroids(object_list)
# Get/Read parameters
object_list_param = rospy.get_param('/object_list')
# loop though the object list
dict_list = []
for i in range(len(object_list_param)):
# Parse parameters into individual variables
object_name = String()
object_name.data = object_list_param[i]['name']
object_group = object_list_param[i]['group']
# Get the PointCloud for a given object and obtain it's centroid
index = find_object_index(object_list_param[i]['name'], object_labels)
# -1 means we never found this object
if index == -1:
continue
centroid = object_centroids[index]
arm = String()
# Assign the arm to be used for pick_place
if object_group == "green":
arm.data = "right"
else:
arm.data = "left"
# Create 'pick_pose' for the object
pick_pose = Pose()
pick_pose.position.x = centroid[0]
pick_pose.position.y = centroid[1]
pick_pose.position.z = centroid[2] + 0.01
# no information in yaml file, or course info, so I am assuming zero
pick_pose.orientation.x = 0
pick_pose.orientation.y = 0
pick_pose.orientation.z = 0
# create 'place_pose' for the object
place_pose = Pose()
# no information in yaml file, or course info, so I am assuming zero
place_pose.orientation.x = 0
place_pose.orientation.y = 0
place_pose.orientation.z = 0
# set position based on the target box
if object_group == "green":
place_pose.position.x = PLACE_GREEN_X
place_pose.position.y = PLACE_GREEN_Y
place_pose.position.z = PLACE_GREEN_Z
else:
place_pose.position.x = PLACE_RED_X
place_pose.position.y = PLACE_RED_Y
place_pose.position.z = PLACE_RED_Z
yaml_dict = make_yaml_dict(TEST_SCENE_NUM, arm, object_name, pick_pose, place_pose)
dict_list.append(yaml_dict)
# send the full collision point cloud
collision_map = []
collision_list = collision_cloud.getCollisionClouds()
table_list = pointCloudToList(object_table)
object_collision_list = create_collision_list_from_objects(object_list, index)
collision_map.extend(table_list)
collision_map.extend(collision_list)
collision_map.extend(object_collision_list)
collision_map_pcl = pcl.PointCloud()
collision_map_pcl.from_list(collision_map)
color_list = get_color_list(2)
collision_map_pcl = XYZ_to_XYZRGB(collision_map_pcl, color_list[0])
collision_map_ros = pcl_to_ros(collision_map_pcl)
sensor_msg_pub.publish(collision_map_ros)
# # Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
# Call the pick_place service
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
resp = pick_place_routine(TEST_SCENE_NUM, object_name, arm, pick_pose, place_pose)
print ("Response: ", resp.success)
#print ("HELLO WORLD")
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def ros_deepsort_callback(self, ros_data):
start = time.time()
#convert ros compressed image message to opencv
if self.args.service:
np_arr = np.fromstring(ros_data.img.data, np.uint8)
elif self.args.img_topic:
np_arr = np.fromstring(ros_data.data, np.uint8)
ori_im = cv2.imdecode(np_arr, flags=cv2.IMREAD_COLOR)
im = cv2.cvtColor(ori_im, cv2.COLOR_BGR2RGB)
#skip frame per frame interval
self.idx_frame += 1
if self.idx_frame % self.args.frame_interval:
return
# do detection
bbox_xywh, cls_conf, cls_ids = self.detector(im)
# select person class
mask = cls_ids == 0
bbox_xywh = bbox_xywh[mask]
# bbox dilation just in case bbox too small, delete this line if using a better pedestrian detector
bbox_xywh[:, 3:] *= 1.2
cls_conf = cls_conf[mask]
# do tracking
outputs = self.deepsort.update(bbox_xywh,
cls_conf,
im,
tracking_target=self.idx_tracked)
# if detection present draw bounding boxes
if len(outputs) > 0:
bbox_tlwh = []
self.bbox_xyxy = outputs[:, :4]
# detection indices
self.identities = outputs[:, -1]
ori_im = draw_boxes(ori_im, self.bbox_xyxy, self.identities)
for bb_xyxy in self.bbox_xyxy:
bbox_tlwh.append(self.deepsort._xyxy_to_tlwh(bb_xyxy))
end = time.time()
#draw frame count
font = cv2.FONT_HERSHEY_SIMPLEX
bottomLeftCornerOfText = (10, 500)
fontScale = 1
fontColor = (255, 255, 255)
lineType = 2
frame_count = ("Frame no: %d" % self.idx_frame)
cv2.putText(ori_im, frame_count, bottomLeftCornerOfText, font,
fontScale, fontColor, lineType)
#draw tracking number
if self.idx_tracked:
tracking_str = ("Tracking: %d" % self.idx_tracked)
else:
tracking_str = ("Tracking: None")
bottomLeftCornerOfText = (10, 550)
cv2.putText(ori_im, tracking_str, bottomLeftCornerOfText, font,
fontScale, fontColor, lineType)
#### Create CompressedIamge ####
msg = CompressedImage()
msg.header.stamp = rospy.Time.now()
msg.format = "jpeg"
msg.data = np.array(cv2.imencode('.jpg', ori_im)[1]).tostring()
# Publish new image
self.image_pub.publish(msg)
if self.args.save_results:
self.writer.write(ori_im)
# logging
self.logger.info("frame: {}, time: {:.03f}s, fps: {:.03f}, detection numbers: {}, tracking numbers: {}" \
.format(self.idx_frame, end-start, 1/(end-start), bbox_xywh.shape[0], len(outputs)))
"""publishing to topics"""
#publish detection identities
identity_msg = Int32MultiArray(data=self.identities)
self.detections_pub.publish(identity_msg)
#publish if target present
if len(outputs) == 0 or self.idx_tracked is None:
self.target_present_pub.publish(Bool(False))
elif len(outputs) > 0 and self.idx_tracked:
self.target_present_pub.publish(Bool(True))
#publish angle and xy data
if self.idx_tracked is not None:
x_center = (self.bbox_xyxy[0][0] + self.bbox_xyxy[0][2]) / 2
y_center = (self.bbox_xyxy[0][1] + self.bbox_xyxy[0][3]) / 2
pixel_per_angle = im.shape[1] / self.camera_fov
x_center_adjusted = x_center - (im.shape[1] / 2)
#print(x_center_adjusted)
angle = x_center_adjusted / pixel_per_angle
#print(angle)
if self.args.img_topic:
self.bbox_pub.publish(x_center, y_center, 0)
self.angle_pub.publish(angle)
self.target_indice_pub.publish(Int32(self.idx_tracked))
msg = Point()
msg.x = x_center
msg.y = y_center
msg.z = 0
return msg
#publish if target initialized
else:
self.target_indice_pub.publish(Int32(self.idx_tracked))
msg = Point()
msg.x = 0
msg.y = 0
msg.z = 0
return msg
def pan_left(self, deg):
pa = rospy.get_param("/robot/get_pan")
pn = pa + deg
v = Int32(pn)
self.blackboard["pub_pan"].publish(v)
def pr2_mover(object_list):
world = 1
dropbox = rospy.get_param('/dropbox')
box = {}
for i, db in enumerate(dropbox):
data = {}
data["arm"] = db['name']
data["position"] = Pose()
data["position"].position.x = db['position'][0]
data["position"].position.y = db['position'][1]
data["position"].position.z = db['position'][2]
box[db['group']] = data
# TODO: Initialize variables
object_list_param = rospy.get_param('/object_list')
# TODO: Get/Read parameters
# TODO: Parse parameters into individual variables
# TODO: Rotate PR2 in place to capture side tables for the collision map
labels = []
centroids = {}
for object in object_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids[object.label] = Pose()
position = (np.mean(points_arr, axis=0)[:3])
centroids[object.label].position.x = np.asscalar(position[0])
centroids[object.label].position.y = np.asscalar(position[1])
centroids[object.label].position.z = np.asscalar(position[2])
dict_list = []
for i, object in enumerate(object_list_param):
# TODO: Loop through the pick list
obj_name = object['name']
obj_group = object['group']
test_scene_num = Int32()
test_scene_num.data = world
object_name = String()
object_name.data = obj_name
arm_name = String()
arm_name.data = box[obj_group]['arm']
pick_pose = centroids[obj_name]
place_pose = box[obj_group]['position']
# TODO: Get the PointCloud for a given object and obtain it's centroid
# TODO: Create 'place_pose' for the object
# TODO: Assign the arm to be used for pick_place
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
'''
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE)
print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
'''
# TODO: Output your request parameters into output yaml file
send_to_yaml("output" + str(world) + ".yaml", dict_list)
def pr2_mover(object_list):
# TODO: Initialize variables
object_list_param = []
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
#rospy.loginfo('object_list_param {}'.format(object_list_param))
# TODO: Parse parameters into individual variables
labels = []
centroids = [] # to be list of tuples (x, y, z)
detected_object_dict = {}
for object in object_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
if not object.label in detected_object_dict:
detected_object_dict.update(
{object.label: np.mean(points_arr, axis=0)[:3]})
#rospy.loginfo('centroid {}'.format(np.mean(points_arr, axis=0)))
#print(detected_object_dict)
test_scene_num = Int32()
test_scene_num.data = rospy.get_param("test_scene_num")
dropbox_dict = {}
for i in range(len(dropbox_param)):
dropbox_dict.update({
dropbox_param[i]['group']:
[dropbox_param[i]['name'], dropbox_param[i]['position']]
})
# TODO: Rotate PR2 in place to capture side tables for the collision map
#rate = rospy.Rate(1000)
#pr2_world_joint_pub.publish(math.pi/2)
#rate.sleep()
##collision_map_pub.publish(pcl_msg)
#pr2_world_joint_pub.publish(-math.pi/2)
#rate.sleep()
#rate.sleep()
#pr2_world_joint_pub.publish(0.0)
#rate.sleep()
# TODO: Loop through the pick list
dict_list = []
for i in range(len(object_list_param)):
# TODO: Get the PointCloud for a given object and obtain it's centroid
object_name = String()
object_name.data = object_list_param[i]['name']
pick_pose = Pose()
centroid = detected_object_dict.get(object_name.data, [0, 0, 0])
pick_pose.position.x = float(centroid[0])
pick_pose.position.y = float(centroid[1])
pick_pose.position.z = float(centroid[2])
#pick_pose.orientation = [0,0,0,0]
#rospy.loginfo('detected_object_dict {}'.format(detected_object_dict.get(object_name.data, [0,0,0])))
# TODO: Create 'place_pose' for the object
place_pose = Pose()
dropbox_pos = dropbox_dict.get(object_list_param[i]['group'])[1]
place_pose.position.x = float(dropbox_pos[0])
place_pose.position.y = float(dropbox_pos[1])
place_pose.position.z = float(dropbox_pos[2])
# TODO: Assign the arm to be used for pick_place
arm_name = String()
arm_name.data = dropbox_dict.get(object_list_param[i]['group'])[0]
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
# Populate various ROS messages
#rospy.loginfo('ROS msg {}, {}, {}, {}, {}'.format(test_scene_num, arm_name, object_name, pick_pose, place_pose) )
#print(object_name.data)
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
#resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE)
resp = pick_place_routine(test_scene_num, object_name, arm_name,
pick_pose, place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def pr2_mover(object_list):
# TODO: Initialize variables
labels = []
centroids = []
dict_list = []
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_filename = '/home/robond/catkin_ws/src/RoboND-Perception-Project/pr2_robot/config/dropbox.yaml'
dropbox_dict = None
with open(dropbox_filename, "r") as stream:
dropbox_dict = yaml.load(stream)
launch_filename = '/home/robond/catkin_ws/src/RoboND-Perception-Project/pr2_robot/launch/pick_place_project.launch'
world_name = "test3.world"
"""
with open(launch_filename, "r") as stream:
element = et.parse(stream)
nodes = element.findall('./launch/include/arg')
for n in nodes:
print(n.text)
if n.attrib('name') == 'world_name':
world_name = n.attrib('value')
break
"""
# TODO: Parse parameters into individual variables
print("World Name: " + world_name)
test_scene_num = re.findall(r'\d', world_name)[0] # no error handling
TEST_SCENE_NUM = Int32()
TEST_SCENE_NUM.data = int(test_scene_num)
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
for i in range(0, len(object_list_param)):
object_name = object_list_param[i]['name']
object_group = object_list_param[i]['group']
# TODO: Get the PointCloud for a given object and obtain it's centroid
do = find_object_by_name(object_list, object_name)
points_arr = ros_to_pcl(do.cloud).to_array()
centroid = np.mean(points_arr, axis=0)[0:3]
centroid_np_array = np.array([
np.asscalar(centroid[0]),
np.asscalar(centroid[1]),
np.asscalar(centroid[2])
])
labels.append(do.label)
centroids.append(centroid_np_array)
OBJECT_NAME = String()
OBJECT_NAME.data = object_name
PICK_POSE = Pose()
PICK_POSE.position.x = np.asscalar(centroid[0])
PICK_POSE.position.y = np.asscalar(centroid[1])
PICK_POSE.position.z = np.asscalar(centroid[2])
# TODO: Create 'place_pose' for the object
place_position = dropbox_dict['dropbox'][1][
'position'] if object_group == 'green' else dropbox_dict[
'dropbox'][0]['position']
PLACE_POSE = Pose(
) # Assuming green/right always the second element in the array
PLACE_POSE.position.x = place_position[0]
PLACE_POSE.position.y = place_position[1]
PLACE_POSE.position.z = place_position[2]
# TODO: Assign the arm to be used for pick_place
WHICH_ARM = String()
WHICH_ARM.data = 'right' if object_group == 'green' else 'left'
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(TEST_SCENE_NUM, WHICH_ARM, OBJECT_NAME,
PICK_POSE, PLACE_POSE)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM,
PICK_POSE, PLACE_POSE)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def control():
msg = Int32()
msg.data = 16
#pub.publish(msg)
return render_template("control.html")
def start_single(self, people_id):
self.num_run_pub.publish(Int32(np.mean(np.asarray(self.num_reps))))
self.controller.start_single(people_id)
def pr2_mover(object_list):
# calculate centroids for detected objects and store in list
labels = []
centroids = [] # to be list of tuples (x, y, z)
for object in object_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# TODO: Initialize variables
yaml_dict_list = []
test_scene_num = Int32()
object_name = String()
which_arm = String()
pick_pose = Pose()
place_pose = Pose()
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
left_place_pose = dropbox_param[0]['position']
right_place_pose = dropbox_param[1]['position']
# TODO: Rotate PR2 in place to capture side tables for the collision map
detected_labels = [object.label for object in object_list]
# TODO: Loop through the pick list
for i in range(len(
object_list_param)): #iterate over all objects in the pick list
print i
if object_list_param[i]['name'] in detected_labels:
# print object_list_param[i]['name']
index = detected_labels.index(object_list_param[i]['name'])
object_name.data = object_list_param[i]['name']
test_scene_num.data = 3
which_arm.data = object_list_param[i]['group']
if (which_arm.data == 'red'):
place_pose.position.x = left_place_pose[0]
place_pose.position.y = left_place_pose[1]
place_pose.position.z = left_place_pose[2]
print 'left'
else:
place_pose.position.x = right_place_pose[0]
place_pose.position.y = right_place_pose[1]
place_pose.position.z = right_place_pose[2]
print 'right'
#print(centroids[index])
pick_pose.position.x = np.asscalar(centroids[index][0])
pick_pose.position.y = np.asscalar(centroids[index][1])
pick_pose.position.z = np.asscalar(centroids[index][2])
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict_list.append(
make_yaml_dict(test_scene_num, which_arm, object_name,
pick_pose, place_pose))
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
print 'service wait completed'
try:
pick_place_routine = rospy.ServiceProxy(
'pick_place_routine', PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name,
which_arm, pick_pose, place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
if __name__ == "__main__":
sample_rate = 5 # Hz
rate_pub = rospy.Publisher(topic_set_rate, Int32, queue_size=10)
bridge_enable_pub = rospy.Publisher(topic_bridge_enable,
String,
queue_size=1,
latch=True)
motors_pub = rospy.Publisher(topic_cmd_vel, Twist, queue_size=10)
sonar_sub = rospy.Subscriber(topic_sonars_response, Sonar_data,
data_callback)
rospy.init_node("obstacle_avoider_simple")
rate_pub.publish(Int32(sample_rate))
bridge_enable_msg = String()
bridge_enable_msg.data = "run"
bridge_enable_pub.publish(bridge_enable_msg)
rospy.loginfo("Published enable comd")
rate = rospy.Rate(sample_rate)
while not rospy.is_shutdown():
direction, dist = get_direction()
twist = set_Twist_msg(direction)
rospy.loginfo("X: {}, Z: {}".format(twist.linear.x, twist.angular.z))
motors_pub.publish(twist)
rate.sleep()
import rospy
from std_msgs.msg import Int32
i = Int32()
def fibonaci(var):
[i, j, k] = [0, 1, 0]
while (k < var):
k = i + j
i = j
j = k
pub.publish(k)
rate.sleep()
while not rospy.is_shutdown():
rospy.init_node('node_name')
pub = rospy.Publisher("topic", Int32, queue_size=50)
rate = rospy.Rate(1)
limit = input(" Enter the limit ")
fibonaci(limit)
def publish_opcode(self, event=None, op=None):
data = Int32(data=op)
self.auto_dig_publisher.publish(data)
def callback(msg):
doubled = Int32()
doubled.data = msg.data * 2
print(doubled.data)
pub.publish(doubled)
def pr2_mover(object_list):
# TODO: Initialize variables
labels = []
centroids = []
obj_label = {}
test_scene_num = Int32()
test_scene_num.data = 2
dict_list = []
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variable
box_left = dropbox_param[0]['position']
box_right = dropbox_param[1]['position']
for object in object_list:
labels.append(object.label)
print(labels)
points_arr = ros_to_pcl(object.cloud).to_array()
# TODO: Get the PointCloud for a given object and obtain it's centroid
centroid = np.mean(points_arr, axis=0)[:3]
centroids.append(centroid)
obj_label[object.label] = centroid
for i in range(0, len(object_list_param)):
object_name.data = object_list_param[i]['name']
object_group = object_list_param[i]['group']
pick_pose.position.x = np.asscalar(obj_label[object_name.data][0])
pick_pose.position.y = np.asscalar(obj_label[object_name.data][1])
pick_pose.position.z = np.asscalar(obj_label[object_name.data][2])
# TODO: Assign the arm to be used for pick_place
if object_list_param[i]['group'] == 'red':
arm_name.data = 'left'
else:
arm_name.data = 'right'
if arm_name.data == 'left':
box_target = box_left
else:
box_target = box_right
place_pose.position.x = box_target[0]
place_pose.position.y = box_target[1]
place_pose.position.z = box_target[2]
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
'''
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name, arm_name, pick_pose, place_pose)
print ("Response: ",resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s"%e
'''
# TODO: Output your request parameters into output yaml file
send_to_yaml("output_2.yaml", dict_list)
rospy.loginfo("yaml sent")
def pose_cb(self, pose):
if self.waypoints is None:
rospy.error('No base_waypoints have been received by master')
return
if not self.dbw_enabled:
return
self.current_pose = pose
# Compute the index of the waypoint closest to the current pose.
closest_wp_idx = helper.next_waypoint_index_kdtree(
self.current_pose.pose, self.waypoints_kdtree)
# Find the closest stop line position if we have to stop the car.
_, stop_line_idx = self.stop_line_positions_kdtree.query(
[pose.pose.position.x, pose.pose.position.y])
stop_line_positions = self.stop_line_positions[stop_line_idx]
# Find the closest waypoint index for the stop line position
stop_line_pose = PoseStamped()
stop_line_pose.pose.position.x = stop_line_positions[0]
stop_line_pose.pose.position.y = stop_line_positions[1]
stop_line_pose.pose.orientation = pose.pose.orientation
stop_line_waypoint_idx = helper.next_waypoint_index_kdtree(
stop_line_pose.pose, self.waypoints_kdtree)
self.stop_line_waypoint_pub.publish(Int32(stop_line_waypoint_idx))
if closest_wp_idx == stop_line_waypoint_idx or self.current_velocity == 0.0:
rospy.logerr(
'Stop Line waypoint reached. Deceleration Sequence Stopped. Current Vel: %s',
self.current_velocity)
self.deceleration_started = False
self.deceleration_waypoints = None
# If the light is RED or YELLOW then slowly decrease the speed.
if self.current_traffic_light is not None:
rospy.logwarn(
'Light color: %s',
helper.get_traffic_light_color(
self.current_traffic_light.state))
if self.current_traffic_light.state == 0 or self.current_traffic_light.state == 1:
if helper.deceleration_rate(
self.current_velocity,
self.distance(self.waypoints, closest_wp_idx,
stop_line_waypoint_idx)) > 0.1:
if not self.deceleration_started:
rospy.logwarn('Deceleration Sequence Started')
new_waypoints = self.set_velocity_leading_to_stop_point(
closest_wp_idx, stop_line_waypoint_idx)
self.deceleration_started = True
self.deceleration_waypoints = new_waypoints
else:
rospy.logwarn(
'Using DECELERATION WAYPOINTS CALCULATED BEFORE')
new_waypoints = self.deceleration_waypoints
elif closest_wp_idx != stop_line_waypoint_idx:
# Simulate the behavior of a green light
new_waypoints = self.behavior_lights_green(closest_wp_idx)
else:
rospy.logerr(
'NOT PUBLISHING ANYTHING SINCE ASSUMING CAR IS AT STOP'
)
return # Do not publish anything. The car is at a STOP and we don't need to do anything
else:
# Lights are green
new_waypoints = self.behavior_lights_green(closest_wp_idx)
else:
new_waypoints = self.behavior_lights_green(closest_wp_idx)
if stop_line_waypoint_idx - closest_wp_idx < 5:
rospy.logerr('Current Waypoint: %s Current Velocity: %s',
closest_wp_idx, self.current_velocity)
for i in range(stop_line_waypoint_idx - 5,
stop_line_waypoint_idx + 1):
rospy.loginfo('%s, %s', i,
new_waypoints[i].twist.twist.linear.x)
# Find number of waypoints ahead dictated by LOOKAHEAD_WPS. However, if the car is stopped then don't accelerate
next_wps = new_waypoints[closest_wp_idx + 1:closest_wp_idx +
LOOKAHEAD_WPS]
self.publish(next_wps)
def pr2_mover(object_list):
# TODO: Initialize variables
test_scene_num = Int32()
arm_name = String()
object_name = String()
object_group = String()
pick_pose = Pose()
place_pose = Pose()
# TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# TODO: Parse parameters into individual variables
object_names = [
object_list_param[i]['name'] for i in range(0, len(object_list_param))
]
object_groups = [
object_list_param[i]['group'] for i in range(0, len(object_list_param))
]
dropbox_names = [
dropbox_param[i]['name'] for i in range(0, len(dropbox_param))
]
dropbox_groups = [
dropbox_param[i]['group'] for i in range(0, len(dropbox_param))
]
dropbox_positions = [
dropbox_param[i]['position'] for i in range(0, len(dropbox_param))
]
# TODO: Rotate PR2 in place to capture side tables for the collision map
# TODO: Loop through the pick list for object labels and centroids
labels = []
centroids = [] # to be list of tuples (x, y, z)
for object in object_list:
# TODO: Get the PointCloud for a given object and obtain it's centroid
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# TODO: ensure test_scene_num is correct for the scene being simulated
test_scene_num.data = int(
3) # change this to suit the world being simulated
# TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
dict_list = []
for i in range(0, len(object_list_param)):
# Populate various ROS messages
object_name.data = str(labels[i])
# TODO: Create 'pick_pose' for the object
pick_pose.position.x = np.asscalar(centroids[i][0])
pick_pose.position.y = np.asscalar(centroids[i][1])
pick_pose.position.z = np.asscalar(centroids[i][2])
# TODO: Assign the arm to be used for pick_place
if object_groups[i] == 'red':
arm_name.data = str('left')
else:
arm_name.data = str('right')
# TODO: Create 'place_pose' for the object
place_pose.position.x = dropbox_positions[dropbox_groups.index(
object_groups[i])][0]
place_pose.position.y = dropbox_positions[dropbox_groups.index(
object_groups[i])][1]
place_pose.position.z = dropbox_positions[dropbox_groups.index(
object_groups[i])][2]
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# TODO: Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, object_name, arm_name,
pick_pose, place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def pr2_mover(object_list):
# Initialize variables
test_scene_num = Int32()
test_scene_num.data = 3
object_name = String()
object_group = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
yaml_dict_list = []
# Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# Parse parameters into individual variables
labels = []
centroids = [] # to be list of tuples (x, y, z)
for object in object_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
#print labels
#print centroids
# TODO: Rotate PR2 in place to capture side tables for the collision map
print len(object_list_param)
# Loop through the pick list
for i in range(len(object_list_param)):
object_name.data = object_list_param[i]['name']
object_group.data = object_list_param[i]['group']
# Get the PointCloud for a given object and obtain it's centroid
idx = labels.index(object_name.data)
pick_pose.position.x = np.asscalar(centroids[idx][0])
pick_pose.position.y = np.asscalar(centroids[idx][1])
pick_pose.position.z = np.asscalar(centroids[idx][2])
# Create 'place_pose' for the object & # assign the arm to be used for pick_place
if (object_group.data == 'green'):
position = [0, -0.71, 0.605]
arm_name.data = 'right'
else:
position = [0, 0.71, 0.605]
arm_name.data = 'left'
place_pose.position.x = position[0]
place_pose.position.y = position[1]
place_pose.position.z = position[2]
# Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
yaml_dict_list.append(yaml_dict)
# Wait for 'pick_place_routine' service to come up
rospy.wait_for_service('pick_place_routine')
try:
pick_place_routine = rospy.ServiceProxy('pick_place_routine',
PickPlace)
# Insert your message variables to be sent as a service request
resp = pick_place_routine(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
print("Response: ", resp.success)
except rospy.ServiceException, e:
print "Service call failed: %s" % e
def update(self):
for i, pub in enumerate(self.enc_publishers):
if pub is None:
continue
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
response = roboclaw.ReadEncM1(address)
elif self.parameters[i]['motor_num'] == 2:
response = roboclaw.ReadEncM2(address)
else:
continue
msg = Int32()
msg.data = int(response[1])
pub.publish(msg)
for i, t in enumerate(self.timeouts):
if self.enc_publishers[i] is None:
continue
if int(round(time.time() * 1000)) - t > self.TIMEOUT_TIME:
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
roboclaw.ForwardBackwardM1(address, 64)
elif self.parameters[i]['motor_num'] == 2:
roboclaw.ForwardBackwardM2(address, 64)
else:
continue
'''
for i, pwm in enumerate(self.pwms):
if pwm is None:
continue
if self.parameters[i] is not None:
address = self.parameters[i]['address']
if self.parameters[i]['motor_num'] == 1:
roboclaw.ForwardBackwardM1(address, pwm)
elif self.parameters[i]['motor_num'] == 2:
roboclaw.ForwardBackwardM2(address, pwm)
self.pwms[i] = None
'''
for i, vel in enumerate(self.velocities):
if vel is None:
continue
#print('found_vel: ' + str(vel))
if self.parameters[i] is not None:
address = self.parameters[i]['address']
#print('address: ' + str(address))
if self.parameters[i]['motor_num'] == 1:
#print('setting m1 speed')
if roboclaw.SpeedM1(address, vel):
#print('m1_set')
pass
else:
#print('m1_failed')
pass
elif self.parameters[i]['motor_num'] == 2:
#print('setting m2 speed')
if roboclaw.SpeedM2(address, vel):
#print('m1_set')
pass
else:
#print('m1_failed')
pass
self.velocities[i] = None
'''
def RealShoot(self, power, pos) : msg = Int32() msg.data = power self.shoot_pub.publish(msg)
def pr2_mover(object_list):
# Initialize variables
test_scene_num = Int32()
arm_name = String()
object_name = String()
pick_pose = Pose()
place_pose = Pose()
dict_list = []
# Get/Read parameters
object_list_param = rospy.get_param('/object_list')
#print(object_list_param)
dropbox_param = rospy.get_param('/dropbox')
test_scene_num.data = 2
# Rotate PR2 in place to capture side tables for the collision map
#pr2_base_mover_pub.publish(1.57)
#print('moving left')
#rospy.sleep(5.0)
#pr2_base_mover_pub.publish(-1.57)
#print('moving right')
#rospy.sleep(5.0)
#pr2_base_mover_pub.publish(0)
#rospy.sleep(5.0)
# Parse parameters into individual variables
labels = []
centroids = []
for object in object_list:
labels.append(object.label)
points_arr = ros_to_pcl(object.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
#print(object.label, points_arr)
#print(object.label)
# Loop through the pick list
#print(len(object_list_param))
for i in range(0, len(object_list_param)):
object_name.data = object_list_param[i]['name']
object_group = object_list_param[i]['group']
# Get the PointCloud for a given object and obtain it's centroid
try:
index = labels.index(object_name.data)
except ValueError:
print('Object not detected')
continue
pick_pose.position.x = np.asscalar(centroids[index][0])
pick_pose.position.y = np.asscalar(centroids[index][1])
#print(index, pick_pose.position.y)
pick_pose.position.z = np.asscalar(centroids[index][2])
# Create 'place_pose' for the object
for j in range(0, len(dropbox_param)):
if object_group == dropbox_param[j]['group']:
place_pose.position.x = dropbox_param[j]['position'][0]
place_pose.position.y = dropbox_param[j]['position'][1]
place_pose.position.z = dropbox_param[j]['position'][2]
# Assign the arm to be used for pick_place
if object_group == 'green':
arm_name.data = 'right'
elif object_group == 'red':
arm_name.data = 'left'
# Create a list of dictionaries
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name,
pick_pose, place_pose)
#print(yaml_dict)
print(object_name, pick_pose.position.y)
dict_list.append(yaml_dict)
# Place item (disabled)
# rospy.wait_for_service('pick_place_routine')
# try:
# pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# Message variables to be sent as a service request
# resp = pick_place_routine(test_scene_num,object_name,arm_name,pick_pose,place_pose)
# print ("Response: ",resp.success)
# except rospy.ServiceException, e:
# print "Service failed: %s"%e
filename = 'output_' + str(test_scene_num.data) + '.yaml'
send_to_yaml(filename, dict_list)
return
def pcl_callback(pcl_msg):
# Exercise-2 TODOs:
# TODO: Convert ROS msg to PCL data
cloud = ros_to_pcl(pcl_msg)
# TODO: Statistical Outlier Filtering
outlier_filter = cloud.make_statistical_outlier_filter()
outlier_filter.set_mean_k(100)
outlier_filter.set_std_dev_mul_thresh(0.003)
cloud_filtered = outlier_filter.filter()
# pcl.save(cloud_filtered, "project_table_scene.pcd")
# # TODO: Voxel Grid Downsampling
vox = cloud_filtered.make_voxel_grid_filter()
LEAF_SIZE = 0.002
vox.set_leaf_size(LEAF_SIZE, LEAF_SIZE, LEAF_SIZE)
cloud_filtered = vox.filter()
# pcl.save(cloud_filtered, "voxel_downsampled.pcd")
# # TODO: PassThrough Filter
passthrough_z= cloud_filtered.make_passthrough_filter()
passthrough_z.set_filter_field_name("z")
passthrough_z.set_filter_limits(0.62, 1.2)
cloud_filtered_z = passthrough_z.filter()
passthrough_y= cloud_filtered_z.make_passthrough_filter()
passthrough_y.set_filter_field_name("y")
passthrough_y.set_filter_limits(-0.50, 0.3)
cloud_filtered = passthrough_y.filter()
# pcl.save(cloud_filtered, "pass_through_filtered.pcd")
# # TODO: RANSAC Plane Segmentation
seg = cloud_filtered.make_segmenter()
seg.set_model_type(pcl.SACMODEL_PLANE)
seg.set_method_type(pcl.SAC_RANSAC)
max_distance = 0.08
seg.set_distance_threshold(max_distance)
inliers, coefficients = seg.segment()
# # TODO: Extract inliers and outliers
cloud_objects = cloud_filtered.extract(inliers, negative=False)
# pcl.save(cloud_objects, "extracted_inliers.pcd")
# # Extract outliers
cloud_table = cloud_filtered.extract(inliers, negative=True)
# pcl.save(cloud_table, "extracted_outliers.pcd")
# # # TODO: Euclidean Clustering
white_cloud = XYZRGB_to_XYZ(cloud_objects)
tree = white_cloud.make_kdtree()
# TODO: Create Cluster-Mask Point Cloud to visualize each cluster separately
ec = white_cloud.make_EuclideanClusterExtraction()
ec.set_ClusterTolerance(0.02)
ec.set_MinClusterSize(300)
ec.set_MaxClusterSize(10000)
# Search the k-d tree for clusters
ec.set_SearchMethod(tree)
# Extract indices for each of the discovered clusters
cluster_indices = ec.Extract()
#Assign a color corresponding to each segmented object in scene
cluster_color = get_color_list(len(cluster_indices))
color_cluster_point_list = []
for j, indices in enumerate(cluster_indices):
for i, indice in enumerate(indices):
color_cluster_point_list.append([white_cloud[indice][0],
white_cloud[indice][1],
white_cloud[indice][2],
rgb_to_float(cluster_color[j])])
#Create new cloud containing all clusters, each with unique color
cluster_cloud = pcl.PointCloud_PointXYZRGB()
cluster_cloud.from_list(color_cluster_point_list)
# pcl.save(cluster_cloud, "cluster_cloud.pcd")
# TODO: Convert PCL data to ROS messages
ros_cloud_objects = pcl_to_ros(cloud_objects)
ros_cloud_table = pcl_to_ros(cloud_table)
ros_cluster_cloud = pcl_to_ros(cluster_cloud)
# TODO: Publish ROS messages
pcl_objects_pub.publish(ros_cloud_objects)
pcl_table_pub.publish(ros_cloud_table)
pcl_cluster_pub.publish(ros_cluster_cloud)
# Exercise-3 TODOs:
detected_objects_labels = []
detected_objects = []
for index, pts_list in enumerate(cluster_indices):
# Grab the points for the cluster from the extracted outliers (cloud_objects)
pcl_cluster = cloud_objects.extract(pts_list)
# TODO: convert the cluster from pcl to ROS using helper function
ros_cluster = pcl_to_ros(pcl_cluster)
# Extract histogram features
# TODO: complete this step just as is covered in capture_features.py
chists = compute_color_histograms(ros_cluster, using_hsv=True)
normals = get_normals(ros_cluster)
nhists = compute_normal_histograms(normals)
feature = np.concatenate((chists, nhists))
# hist_bins=64
# chists = compute_color_histograms(ros_cluster, nbins=hist_bins, using_hsv=True)
# normals = get_normals(ros_cluster)
# nhists = compute_normal_histograms(normals, nbins=hist_bins)
# feature = np.concatenate((chists, nhists))
# Make the prediction, retrieve the label for the result
# and add it to detected_objects_labels list
prediction = clf.predict(scaler.transform(feature.reshape(1,-1)))
label = encoder.inverse_transform(prediction)[0]
detected_objects_labels.append(label)
# Publish a label into RViz
label_pos = list(white_cloud[pts_list[0]])
label_pos[2] += .4
object_markers_pub.publish(make_label(label,label_pos, index))
# Add the detected object to the list of detected objects.
do = DetectedObject()
do.label = label
do.cloud = ros_cluster
detected_objects.append(do)
rospy.loginfo('Detected {} objects: {}'.format(len(detected_objects_labels), detected_objects_labels))
# Publish the list of detected objects
# This is the output you'll need to complete the upcoming project!
detected_objects_pub.publish(detected_objects)
# detected_objects_pub.publish(pcl_msg)
# Suggested location for where to invoke your pr2_mover() function within pcl_callback()
# Could add some logic to determine whether or not your object detections are robust
# before calling pr2_mover()
# try:
# pr2_mover(detected_objects_list)
# except rospy.ROSInterruptException:
# pass
# function to load parameters and request PickPlace service
# def pr2_mover(object_list):
# TODO: Initialize variables
test_scene_num = Int32()
object_name = String()
arm_name = String()
pick_pose = Pose()
place_pose = Pose()
dict_list = []
yaml_filename = 'output_1.yaml'
test_scene_num.data = 1
labels = []
centroids = []
for objects in detected_objects:
labels.append(objects.label)
points_arr = ros_to_pcl(objects.cloud).to_array()
centroids.append(np.mean(points_arr, axis=0)[:3])
# # TODO: Get/Read parameters
object_list_param = rospy.get_param('/object_list')
dropbox_param = rospy.get_param('/dropbox')
# # TODO: Rotate PR2 in place to capture side tables for the collision map
# # TODO: Loop through the pick list
# # TODO: Parse parameters into individual variables
for i in range(0, len(object_list_param)):
object_name.data = object_list_param[i]['name']
object_group = object_list_param[i]['group']
# # TODO: Get the PointCloud for a given object and obtain it's centroid
for j in range(0, len(labels)):
if object_name.data == labels[j]:
pick_pose.position.x = np.asscalar(centroids[j][0])
pick_pose.position.y = np.asscalar(centroids[j][1])
pick_pose.position.z = np.asscalar(centroids[j][2])
# # TODO: Create 'place_pose' for the object
for j in range(0, len(dropbox_param)):
if object_group == dropbox_param[j]['group']:
place_pose.position.x = dropbox_param[j]['position'][0]
place_pose.position.y = dropbox_param[j]['position'][1]
place_pose.position.z = dropbox_param[j]['position'][2]
# # TODO: Assign the arm to be used for pick_place
if object_group == 'green':
arm_name.data = 'right'
elif object_group == 'red':
arm_name.data = 'left'
# # TODO: Create a list of dictionaries (made with make_yaml_dict()) for later output to yaml format
yaml_dict = make_yaml_dict(test_scene_num, arm_name, object_name, pick_pose, place_pose)
dict_list.append(yaml_dict)
# # Wait for 'pick_place_routine' service to come up
# rospy.wait_for_service('pick_place_routine')
# try:
# pick_place_routine = rospy.ServiceProxy('pick_place_routine', PickPlace)
# # TODO: Insert your message variables to be sent as a service request
# resp = pick_place_routine(TEST_SCENE_NUM, OBJECT_NAME, WHICH_ARM, PICK_POSE, PLACE_POSE)
# print ("Response: ",resp.success)
# except rospy.ServiceException, e:
# print "Service call failed: %s"%e
# # TODO: Output your request parameters into output yaml file
send_to_yaml(yaml_filename, dict_list)
def publish_mission_state(mission_state): global mission_state_pub mission_state_msg = Int32() mission_state_msg.data = mission_state mission_state_pub.publish(mission_state_msg)
