async def timer_callback(self):
global x, y, th, pos_x, pos_y, ori_th, v_pos_x, v_pos_y, v_ori_th
req = TmsdbGetData.Request()
req.tmsdb.id = 2012
req.tmsdb.sensor = 3001
result = await self.db_client.call_async(req)
var_pos_x = result.tmsdb[0].x
var_pos_y = result.tmsdb[0].y
ori_th = result.tmsdb[0].ry
roll = result.tmsdb[0].rr
pitch = result.tmsdb[0].rp
yaw = result.tmsdb[0].ry
async def calltimercallback(self):
self.calltimer.cancel()
req = TmsdbGetData.Request()
req.tmsdb.id = 2012
req.tmsdb.sensor = 3001
result = await self.db_client.call_async(req)
var_pos_x = result.tmsdb[0].x
var_pos_y = result.tmsdb[0].y
ori_th = result.tmsdb[0].ry
roll = result.tmsdb[0].rr
pitch = result.tmsdb[0].rp
yaw = result.tmsdb[0].ry
async def call_dbreader(self,data):
"""[tms_db_reader] DBからデータを読み取る
"""
req = TmsdbGetData.Request()
req.tmsdb = data
self.future_dbreader = self.cli_dbreader.call_async(req)
await self.future_dbreader
if self.future_dbreader.result() is not None:
res = self.future_dbreader.result().tmsdb
return res
else:
self.get_logger().info('Service "tms_db_reader" call failed %r' % (self.future_dbreader.exception(),))
async def call_dbreader(self, id):
"""[tms_db_reader] DBからデータを読み取る
"""
self.cli_dbreader = self.create_client(TmsdbGetData, 'tms_db_reader', callback_group=self.cb_group)
while not self.cli_dbreader.wait_for_service(timeout_sec=1.0):
self.get_logger().info('service "tms_db_reader" not available, waiting again...')
req = TmsdbGetData.Request()
req.tmsdb.id = id + 100000 # TODO: why add 100000 ?
self.future_dbreader = self.cli_dbreader.call_async(req)
await self.future_dbreader
if self.future_dbreader.result() is not None:
res = self.future_dbreader.result().tmsdb
return res
else:
self.get_logger().info('Service "tms_db_reader" call failed %r' % (self.future_dbreader.exception(),))
async def calltimercallback(self):
global x, y, th, pos_x, pos_y, ori_th, v_pos_x, v_pos_y, v_ori_th, var_pos_x, var_pos_y, v_rotation
self.calltimer.cancel()
print("a")
req = TmsdbGetData.Request()
req.tmsdb.id = 2012
req.tmsdb.sensor = 3001
print("a")
result = await self.db_client.call_async(req)
print("a")
var_pos_x = result.tmsdb[0].x
var_pos_y = result.tmsdb[0].y
ori_th = result.tmsdb[0].ry
print("8")
roll = result.tmsdb[0].rr
pitch = result.tmsdb[0].rp
yaw = result.tmsdb[0].ry
async def control_base(self, command, goal_dis, goal_ang):
global x, y, th, pos_x, pos_y, ori_th, v_pos_x, v_pos_y, v_ori_th
ini_pos_x = v_pos_x
ini_pos_y = v_pos_y
ini_ori_th = v_ori_th
var_pos_x = v_pos_x
var_pos_y = v_pos_y
var_ori_th = v_ori_th
req = TmsdbGetData.Request()
req.tmsdb.id = 2012
req.tmsdb.sensor = 3001
result = await self.db_client.call_async(req)
ini_pos_x = req.response.tmsdb[0].x
ini_pos_y = req.response.tmsdb[0].y
var_pos_x = result.tmsdb[0].x
var_pos_y = result.tmsdb[0].y
if command == 0 or command == 2:
if goal_ang > 0:
while True:
if ini_ori_th + goal_ang > 180:
if (ini_ori_th <= var_ori_th and var_ori_th <= 180
) or (-180 <= var_ori_th and
var_ori_th < ini_ori_th + goal_ang - 360):
var_ori_th = v_ori_th
else:
break
else:
if math.fabs(var_ori_th -
ini_ori_th) < math.fabs(goal_ang):
var_ori_th = v_ori_th
else:
break
elif goal_ang < 0:
while True:
if ini_ori_th + goal_ang < -180:
if (-180 <= var_ori_th and var_ori_th <= ini_ori_th
) or (360 + goal_ang + ini_ori_th < var_ori_th
and var_ori_th <= 180):
var_ori_th = v_ori_th
else:
break
else:
if math.fabs(var_ori_th -
ini_ori_th) < math.fabs(goal_ang):
var_ori_th = v_ori_th
else:
break
if (command == 1 or command == 2):
if goal_dis > 0:
while True:
if distance(ini_pos_x, ini_pos_y, var_pos_x,
var_pos_y) < goal_dis:
var_pos_x = v_pos_x
var_pos_y = v_pos_y
result = await self.db_client.call_async(req)
var_pos_x = result.tmsdb[0].x
var_pos_y = result.tmsdb[0].y
else:
break
return true
async def callback(self, request, response):
global x, y, th, pos_x, pos_y, ori_th, v_pos_x, v_pos_y, v_ori_th
if request.cmd == 0:
if len(request.arg) != 3 or request.arg[0] < 0 or request.arg[
0] > 8000 or request.arg[1] < 0 or request.arg[
1] > 4500 or request.arg[2] < -180 or request.arg[
2] > 180:
self.get_logger().info("case0 : An illegal arguments' type.\n")
response.result = 0
return response
current_x = v_pos_x
current_y = v_pos_y
current_th = v_ori_th
req = TmsdbGetData.Request()
req.tmsdb.id = 2012
req.tmsdb.sensor = sensor
result = await self.db_client.call_async(req)
current_x = result.tmsdb[0].x
current_y = result.tmsdb[0].y
goal_distance = distance(current_x, current_y, request.arg[0],
request.arg[1])
goal_theta = request.arg[2] - current_th
if (goal_theta > 180.0):
goal_theta = goal_theta - 360.0
elif (goal_theta < -180.0):
goal_theta = goal_theta + 360.0
if (goal_distance >= 5 and goal_theta < 2):
command = 1
elif (goal_distance >= 5 and goal_theta >= 2):
command = 2
control_base(command, goal_distance, goal_theta)
response.result = 1
elif request.cmd == 1:
if len(request.arg) != 3 or request.arg[0] < 0 or request.arg[
0] > 8000 or request.arg[1] < 0 or request.arg[
1] > 4500 or request.arg[2] < -180 or request.arg[
2] > 180:
self.get_logger().info("case0 : An illegal arguments' type.\n")
response.result = 0
while goal_theta > 180.0:
goal_theta = goal_theta - 360.0
while goal_theta < -180.0:
goal_theta = goal_theta + 360.0
return response
current_x = v_pos_x
current_y = v_pos_y
current_th = v_ori_th
req = TmsdbGetData.Request()
req.tmsdb.id = 2012
req.tmsdb.sensor = sensor
result = await self.db_client.call_async(req)
current_x = result.tmsdb[0].x
current_y = result.tmsdb[0].y
goal_distance = distance(current_x, current_y, request.arg[0],
request.arg[1])
goal_theta = request.arg[2] - current_th
while goal_theta > 180.0:
goal_theta = goal_theta - 360.0
while goal_theta < -180.0:
goal_theta = goal_theta + 360.0
if len(request.arg) != 2 or request.arg[0] < 0 or request.arg[
0] > 9179 or request.arg[1] < -180 or request.arg[1] > 180:
self.get_logger().info("case0 : An illegal arguments' type.\n")
response.result = 0
return response
if (goal_distance >= 5 and goal_theta < 2):
command = 1
elif (goal_distance >= 5 and goal_theta >= 2):
command = 2
print(
f"command:{command}: goal_dis={request.arg[0]}, goal_arg={request.arg[1]}"
)
control_base(command, goal_distance, goal_theta)
response.result = 1
else:
self.get_logger().info("case0 : An illegal arguments' type.\n")
response.result = 0
return response
pub_tf()
return response
