def saveEditor(data):
print data['name']
db.DeletePose(map=data['name'])
db.DeletePoints(name=data['name'])
for ms in data['zoneData']:
db.InsertPose(name=ms['settings']['name'],
x=ms['settings']['x'] * 0.05,
y=(data['height'] - ms['settings']['y']) * 0.05,
theta=ms['settings']['theta'],
map=data['name'],
type=ms['settings']['type'])
if ('navData' in data):
data_ = db.ImagetoMap(data=data['navData'])
data_ = np.flipud(data['navData'])
db.UpdateMap(name=data['name'],
newvalues={'navdata': db.ImagetoMap(data=data_)})
#print datacvt
if ('wallData' in data):
data__ = db.ImagetoMap(data=data['wallData']['imgdata'])
data__ = np.flipud(data__)
db.UpdateMap(name=data['name'],
newvalues={'walldata': db.ImagetoMap(data=data__)})
db.InsertPoints(name=data['name'], data=data['wallData']['pointdata'])
print("save ok")
def on_message(self, message):
print "ok"
print message
global nameDash
a = json.loads(message) #conver json to python
print a["cmd"]
if (a["cmd"] == "saveDash"):
print a["name"]
db.InsertDashboard(a["name"], a["data"])
if (a["cmd"] == "chooseDash"):
nameDash = a["name"]
if (a["cmd"] == "loadDash"):
dash = db.FindDashboard(nameDash)
for da in dash:
name_dash = da['name']
if (nameDash == name_dash):
#print json.dumps(da['data'])
self.write_message(json.dumps(da['data']))
if (a["cmd"] == "chosenDash"):
self.write_message(nameDash)
if (a["cmd"] == "deleteDash"):
db.DeleteDashboard(a["name"])
os.rmdir(rospkg.RosPack().get_path('rtcrobot_webinterface') +
'/www/assets/img/maps/' + ["name"])
def __save_map(self, map):
rospy.loginfo("Recived a %d X %d map @ %.3f m/pix", map.info.width,
map.info.height, map.info.resolution)
#! Create zero image
#img = np.zeros((map.info.height, map.info.width))
#img[:, :] = [[ord(255) for j in range(map.info.width)] for i in range(map.info.height)]
#! Create image map file - navigation
datacvt = []
for y in range(map.info.height):
for x in range(map.info.width):
px = map.data[x + (map.info.height - y - 1) * map.info.width]
if px >= 0 and px <= self.threshold_free_:
#fout_img.write(chr(254))
datacvt.append(254)
elif px >= self.threshold_occupied_:
#fout_img.write(chr(0))
datacvt.append(0)
else:
#fout_img.write(chr(205))
datacvt.append(205)
dimension = {"width": map.info.width, "height": map.info.height}
nav = np.reshape(datacvt, (map.info.height, map.info.width))
#! Creat image map file - wall
wall = [-1 for i in range(map.info.height * map.info.width)]
db.InsertMap(name=self.mapname_,
dimension=dimension,
navdata=map.data,
walldata=wall)
def spin(self):
r = rospy.Rate(10)
while not rospy.is_shutdown():
if self.__execute:
self.__execute = False
if self.__command.startswith('MOVE'):
#get Pose by name to targetpose
posename = self.__command.replace('MOVE', '').strip()
doc = db.FindPose(name=posename, map=self.__currentmap)
if doc.count() == 0:
self._result.done = False
self._result.description = 'Get pose failed'
self.__isdone = 7
else:
posedata = doc[0]
print posedata
targetpose = Pose(x=posedata['x'],
y=posedata['y'],
theta=posedata['theta'],
posetype=posedata['type'])
self.__MovePose(targetpose)
elif self.__command.startswith('WAIT'):
command = self.__command.split(' ')
if command[1] == 'TIME':
self.__Wait(float(command[2]))
pass
r.sleep()
pass
def __init__(self):
rospy.init_node('rtcrobot_mapserver', anonymous=True)
rospy.loginfo('Map server stated!')
self.frame_id = rospy.get_param('~frame_id', 'map')
self.occupied_thresh = rospy.get_param('~occupied_thresh', 65)
self.free_thresh = rospy.get_param('~free_thresh', 0.196)
self.resolution = rospy.get_param('~resolution', 0.05)
self.pub_map = rospy.Publisher('map',
OccupancyGrid,
latch=True,
queue_size=10)
self.pub_wall = rospy.Publisher('maps/wall',
OccupancyGrid,
latch=True,
queue_size=10)
self.pub_mapmeta = rospy.Publisher('map_metadata',
MapMetaData,
latch=True,
queue_size=10)
self.pub_currentmap = rospy.Publisher('rtcrobot/currentmap',
String,
latch=True,
queue_size=10)
savemapdb.SaveMap()
doc = db.FindActiveMap()
for active in doc:
if self.__loadMap(active['name']):
self.pub_currentmap.publish(active['name'])
break
sv = rospy.Service('/robot_services/switchmap', SwitchMap,
self.svcallback)
pass
def AutoCharge(self):
if self.__robotstate.code == RobotState.CHARGING:
return False
elif self.__robotstate.code == RobotState.READY and self.__enableFindCharger:
myclient = pymongo.MongoClient("mongodb://localhost:27017/")
database = myclient["robotconfig"]
collection = database["common"]
doc = collection.find_one()
self.__percentautochargeenable = doc['autoChargeenable']
if self.battery.percentage < self.__percentautochargeenable:
rospy.logerr('GO TO CHARGER')
#find charge pose
dist_min = 10000
posename =''
for pose in db.FindPose(type=2, map=self.__currentmap):
#calculate distance from robot
d = pow(pose['x'] - self.robotpose.pose.position.x,2)+pow(pose['y'] - self.robotpose.pose.position.y,2)
if d< dist_min:
posename = pose['name']
dist_min = d
rospy.loginfo("Find charger ...%s...", posename)
goal = MissionGoal()
goal.command = 'MOVE ' + posename
self.client.send_goal(goal, active_cb=self.active_cb)
self.__enableFindCharger = False
return True
return False
def deletemap(name):
rospack = rospkg.RosPack()
db.DeleteMap(name=name)
#os.rmdir(rospkg.RosPack().get_path('rtcrobot_webinterface')+'/www/assets/img/maps/' + ["name"])
dir = rospkg.RosPack().get_path(
'rtcrobot_webinterface') + '/www/assets/img/maps/' + name
shutil.rmtree(dir)
pass
def get(self):
name = []
dash = db.FindDashboard()
i = 0
for da in dash:
name_dash = da['name']
name.insert(i, name_dash)
i = i + 1
self.render('../www/dashboard_manager.html', names=name)
def get(self):
map__ = []
map_ = db.FindMap()
i = 0
for ms in map_:
map__.insert(i, ms)
i = i + 1
self.render('../www/maps.html', maps=map__)
def get(self):
arr_name = []
mission = db.FindMission()
i = 0
for ms in mission:
name_mission = ms['name']
arr_name.insert(i, name_mission)
i = i + 1
length = i
self.render('../www/dashboard_create.html',
length=length,
arr_name=arr_name)
def __loadMap(self, mapname='map'):
nav = GetMapResponse()
#// Copy the image data into the map structure
nav.map.info.origin.position.x = 0.0
nav.map.info.origin.position.y = 0.0
nav.map.info.origin.position.z = 0.0
nav.map.info.origin.orientation.x = 0.0
nav.map.info.origin.orientation.y = 0.0
nav.map.info.origin.orientation.z = 0.0
nav.map.info.origin.orientation.w = 1.0
nav.map.info.map_load_time = rospy.Time.now()
nav.map.header.frame_id = self.frame_id
wall = GetMapResponse()
#// Copy the image data into the map structure
wall.map.info.origin.position.x = 0.0
wall.map.info.origin.position.y = 0.0
wall.map.info.origin.position.z = 0.0
wall.map.info.origin.orientation.x = 0.0
wall.map.info.origin.orientation.y = 0.0
wall.map.info.origin.orientation.z = 0.0
wall.map.info.origin.orientation.w = 1.0
wall.map.info.map_load_time = rospy.Time.now()
wall.map.header.frame_id = self.frame_id
maps = db.FindMap(mapname)
if maps.count() > 0:
mapdata = maps[0]
nav.map.info.height = mapdata['dimension']['height']
nav.map.info.width = mapdata['dimension']['width']
nav.map.info.resolution = 0.05
wall.map.info = nav.map.info
nav.map.data = self.__convert(mapdata['navdata'])
self.pub_mapmeta.publish(nav.map.info)
self.pub_map.publish(nav.map)
wall.map.data = np.int8(mapdata['walldata'])
self.pub_wall.publish(wall.map)
rospy.loginfo("Read '%s' map @ %d X %d @ %.3lf m/cell", mapname,
nav.map.info.width, nav.map.info.height,
nav.map.info.resolution)
return True
rospy.loginfo("Can't load map")
return False
def sv_loadcallback(self, request):
if(request.name == ''):
return CallMissionResponse()
doc = db.FindMission(request.name)
if doc.count > 0:
self.lst_command_ = [str(d) for d in doc[0]['actions']]
self.missionstate_.actions = self.lst_command_
self.numcommand = len(self.lst_command_)
self.command_idx = 0
print self.lst_command_
rep = CallMissionResponse()
rep.feedback = True
return rep
return CallMissionResponse()
def post(self, *args):
"""Example handle ajax post"""
mapname = self.get_argument('map')
Map = db.FindMap(name=mapname)
Point = db.FindPoints(name=mapname)
data_navData = []
navdata = []
walldata = []
zoneData = []
pointdata = []
height = 0
width = 0
if Point.count() > 0:
pt = Point[0]
pointdata = pt['data']
pose_ = {"layer": 12, "type": "point", "settings": {"theta": 0}}
fodder_path = rospkg.RosPack().get_path(
'rtcrobot_webinterface') + '/www/assets/img/maps/' + mapname
if (os.path.isdir(fodder_path)):
print('ok')
else:
os.makedirs(fodder_path)
if Map.count() > 0:
mp = Map[0]
height = mp['dimension']['height']
width = mp['dimension']['width']
image = np.reshape(db.MaptoImage(data=mp['navdata']),
(height, width))
#image = np.reshape(db.MaptoImage(data=mp['navdata']),(1000,1000))
image = np.flipud(image)
#image = np.reshape(mp['navdata'],(mp['dimension']['height'],mp['dimension']['width'] ))
cv2.imwrite(fodder_path + '/navigation.png', image)
imgFile = open(fodder_path + '/navigation.png', 'r+')
navdata = base64.standard_b64encode(imgFile.read())
image1 = np.reshape(db.MaptoImage(data=mp['walldata']),
(height, width))
#image = np.reshape(db.MaptoImage(data=mp['navdata']),(1000,1000))
image1 = np.flipud(image1)
#image = np.reshape(mp['navdata'],(mp['dimension']['height'],mp['dimension']['width'] ))
cv2.imwrite(fodder_path + '/wall.png', image1)
imgFile1 = open(fodder_path + '/wall.png', 'r+')
walldata = base64.standard_b64encode(imgFile1.read())
print walldata
pose = db.FindPose(map=mapname)
i = 0
for po in pose:
print po['x']
pose_['settings']['name'] = str(po['name'])
pose_['settings']['x'] = po['x'] / 0.05
pose_['settings']['y'] = po['y'] / 0.05
pose_['settings']['theta'] = po['theta']
pose_['settings']['type'] = po['type']
#p = pose_.deepcopy()
#print pose_
i = i + 1
print i
zoneData.append(deepcopy(pose_))
print zoneData
#print zoneData
data = {
'height': height,
'width': width,
'name': mapname,
'navData': navdata,
'wallData': {
'imgdata': walldata,
'pointdata': pointdata,
},
'zoneData': zoneData
}
self.write(json.dumps(data))
self.finish()
def on_message(self, message):
#print message
#print 'message received %s' % message # recive
# open file json
data = json.loads(message) # convert to string
length = len(data)
file_path = "json/mission.json"
path = rospkg.RosPack().get_path(
'rtcrobot_webinterface') + "/json/mission.json"
file_mission = open(path, "r") # open file
read_file = file_mission.readline()
convert = json.loads(read_file) # convert string to json
list_mission = convert["list"]
length = len(list_mission)
path1 = "/home/mtk/catkin_ws/src/web/json/run_mission.json"
#***************************************************************************
# CREAT NEWMISSION
#***************************************************************************
if (data["command"] == "creat_mission"): # tao mission moi
tem = 0
list_ = {"name": []}
mission = db.FindMission()
for ms in mission:
name = ms['name']
if (data['name_mission'] == name):
print('error')
self.write_message("error")
tem = 1
if (tem != 1):
db.InsertMission(data['name_mission'], data['createby'], '',
'')
self.write_message("ok")
#***************************************************************************
# Display mission on page list mission
#***************************************************************************
if (data["command"] == "list_mission"
): # display list mission for page mission
print("list_mission")
list_ = {"name": []}
mission = db.FindMission()
for ms in mission:
name = ms['name']
list_["name"].insert(0, name)
send_web = json.dumps(list_)
self.write_message(send_web)
#***************************************************************************
#edit Mission
#***************************************************************************
if (data["command"] == "edit_mission"): # edit mission
print("edit_mission")
global mission_edit
mission_edit = data['name']
self.write_message("edit")
#***************************************************************************
#***************************************************************************
if (data["command"] == "display_mission"):
# load mission
mission = db.FindMission()
send = {
'action': [],
'map': [],
'pose': [],
'name_mission': mission_edit
}
list_pose_ = {}
for ms in mission:
name_mission = ms['name']
i = 0
if (mission_edit == name_mission):
for x in ms['actions']:
send['action'].insert(i, x)
i = i + 1
#load map
map_ = db.FindMap()
j = 0
i = 0
for ms in map_:
name_map = ms['name']
send['map'].insert(j, name_map)
j = j + 1
pose_ = []
pose = db.FindPose(map=name_map)
for po in pose:
#print po['name']
pose_.insert(i, po['name'])
i = i + 1
send['pose'].insert(j, pose_)
send_web = json.dumps(send)
self.write_message(send_web)
#***************************************************************************
#***************************************************************************
if (data["command"] == "save_mission"):
print("save_mission")
edit = {'actions': []}
i = 0
for ms in data['action']:
edit['actions'].insert(i, ms)
i = i + 1
db.UpdateMission(name=mission_edit, newvalues=edit)
if (data["command"] == "choose_mission"):
print("choose_mission")
global choose_mission
choose_mission = data['name']
if (data["command"] == "load_choose_mission"):
self.write_message(choose_mission)
#***************************************************************************
# xoa mission
#***************************************************************************
if (data["command"] == "remove_mission"):
mission = db.FindMission()
for ms in mission:
name = ms['name']
if (data['name'] == name):
db.DeleteMission(name)
#***************************************************************************
# display list parameter
#***************************************************************************
if (data["command"] == "setting"):
file_path = rospkg.RosPack().get_path(
'rtcrobot_navigation') + "/config/system.yaml"
with open(file_path) as f:
doc = yaml.load(f)
doc.update({'sound': get_sound.Sounds.loadsounds()})
send_web = json.dumps(doc)
self.write_message(send_web)
#self.write_message(json.dumps(get_sound.Sounds.loadsounds()))
#print get_sound.Sounds.loadsounds()
#t1.start()
#***************************************************************************
# save
#***************************************************************************
if (data["command"] == "save_setting"):
#t1.setDaemon(False)
#print data
file_path = rospkg.RosPack().get_path(
'rtcrobot_navigation') + "/config/system.yaml"
with open(file_path) as f: # load file yaml
doc = yaml.load(f)
lon = data['paramerter']
lon2 = lon['DWBLocalPlanner']
lon3 = lon['light']['state']
for x in range(len(data['paramerter']['DWBLocalPlanner'])):
data_write_DW = float(list(lon2.values())[x])
doc['DWBLocalPlanner'][list(lon2)[x]] = data_write_DW
for x in range(len(data['paramerter']['light']['state'])):
data_write_state = str(list(lon3.values())[x])
doc['light']['state'][list(lon3)[x]] = data_write_state
with open(file_path, 'w') as f:
yaml.dump(doc, f)