def cb_kiwibot_status(self, msg: Kiwibot) -> None:
"""
Callback to update kiwibot state information in visuals
Args:
msg: `Kiwibot` message with kiwibot state information
int8 pos_x # x axis position in the map
int8 pos_y # y axis position in the map
float32 dist # distance traveled by robot
float32 speed # speed m/s
float32 time # time since robot is moving
float32 yaw # time since robot is moving
bool moving # Robot is moving
Returns:
"""
try:
self.kiwibot_state = msg
except Exception as e:
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
printlog(
msg="{}, {}, {}, {}".format(e, exc_type, fname,
exc_tb.tb_lineno),
msg_type="ERROR",
)
def cb_path_planner(self, msg: planner_msg) -> None:
"""
Callback to update path planner state information in visuals
Args:
msg: `planner_msg` message with planner state information
LandMark[] land_marks # landmarks of planner routine
int8[] neighbors # id of closer neighbors
int8 id # id unic landmark identifier
int32 x # x axis position
int32 y # y axis position
float32 distance # in meters
float32 duration # in seconds
float32 difficulty # in seconds
Returns:
"""
try:
self.msg_planner = msg
# Read again background image to update visuals and components
self._win_background = cv2.imread(self._win_background_path)
self._kiwibot_img = cv2.imread(self._kiwibot_img_path,
cv2.IMREAD_UNCHANGED)
self.turn_robot(
heading_angle=float(os.getenv("BOT_INITIAL_YAW", default=0.0)))
self.draw_descriptors(self.msg_planner.land_marks)
except Exception as e:
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
printlog(
msg="{}, {}, {}, {}".format(e, exc_type, fname,
exc_tb.tb_lineno),
msg_type="ERROR",
)
def cb_srv_robot_move(self, request, response) -> Move:
"""
Callback to update kiwibot state information when move
request service
Args:
request: `usr_srvs.srv._move.Move_Request` request of turning
references to move the robot
Returns:
response: `usr_srvs.srv._move.Move_Response` Moving request
successfully completed or not
"""
try:
for wp in request.waypoints:
if self._FORWARE_PRINT_WAYPOINT:
printlog(msg=wp, msg_type="INFO")
# Updating robot status
abs_dist = (np.sqrt(
pow(self.status.pos_x - wp.x, 2) +
pow(self.status.pos_y - wp.y, 2)) * 0.00847619047)
self.status.speed = abs_dist / wp.dt
self.status.dist += abs_dist
self.status.pos_x = wp.x
self.status.pos_y = wp.y
self.status.moving = True
self.status.time += wp.dt
self.pub_bot_status.publish(self.status)
time.sleep(wp.dt)
self.status.speed = 0.0
self.status.moving = False
self.pub_bot_status.publish(self.status)
self.pub_speaker.publish(Int8(data=1))
response.completed = True
except Exception as e:
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
printlog(
msg="{}, {}, {}, {}".format(e, exc_type, fname,
exc_tb.tb_lineno),
msg_type="ERROR",
)
response.completed = False
return response
def cb_srv_robot_turn(self, request, response) -> Turn:
"""
Callback to update kiwibot state information when turning
request service
Args:
request: `usr_srvs.srv._turn.Turn_Request` request of turning
references to turn the robot
Returns:
response: `usr_srvs.srv._turn.Turn_Response` turning request
successfully completed or not
"""
try:
for idx, turn_ref in enumerate(request.turn_ref[:-1]):
if self._TURN_PRINT_WAYPOINT:
printlog(msg=turn_ref, msg_type="INFO")
self.status.speed = 0.0
self.status.yaw += (request.turn_ref[idx + 1].yaw -
request.turn_ref[idx].yaw)
self.status.time += turn_ref.dt
self.status.moving = True
if self.status.yaw >= 360:
self.status.yaw += -360
self.pub_bot_status.publish(self.status)
time.sleep(turn_ref.dt)
self.status.moving = False
self.pub_bot_status.publish(self.status)
response.completed = True
except Exception as e:
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
printlog(
msg="{}, {}, {}, {}".format(e, exc_type, fname,
exc_tb.tb_lineno),
msg_type="ERROR",
)
response.completed = False
return response
def cb_kiwibot_status(self, msg: kiwibot_msg) -> None:
"""
Callback to update kiwibot state information in visuals
Args:
msg: `kiwibot_msg` message with kiwibot state information
int8 pos_x # x axis position in the map
int8 pos_y # y axis position in the map
float32 dist # distance traveled by robot
float32 speed # speed m/s
float32 time # time since robot is moving
float32 yaw # time since robot is moving
bool moving # Robot is moving
Returns:
"""
try:
# rotate robot's image
if self.msg_kiwibot.yaw != msg.yaw:
if not int((msg.yaw - int(msg.yaw)) * 100):
self._kiwibot_img = cv2.imread(self._kiwibot_img_path,
cv2.IMREAD_UNCHANGED)
self.turn_robot(heading_angle=msg.yaw)
else:
move_angle = msg.yaw - self.msg_kiwibot.yaw
self.turn_robot(heading_angle=move_angle)
self.msg_kiwibot = msg
except Exception as e:
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
printlog(
msg="{}, {}, {}, {}".format(e, exc_type, fname,
exc_tb.tb_lineno),
msg_type="ERROR",
)
def __init__(self) -> None:
"""
Class constructor for path planning node
Args:
Returns:
"""
# ---------------------------------------------------------------------
Node.__init__(self, node_name="planner_node")
# Allow callbacks to be executed in parallel without restriction.
self.callback_group = ReentrantCallbackGroup()
# ---------------------------------------------------------------------
# Environment variables for forware and turn profiles
self._TURN_ACELERATION_FC = float(
os.getenv("TURN_ACELERATION_FC", default=0.3))
self._TURN_CRTL_POINTS = float(
os.getenv("TURN_CRTL_POINTS", default=30))
self._FORWARE_ACELERATION_FC = float(
os.getenv("FORWARE_ACELERATION_FC", default=0.3))
self._FORWARE_CRTL_POINTS = float(
os.getenv("FORWARE_CRTL_POINTS", default=30))
self._TURN_TIME = float(os.getenv("TURN_TIME", default=3.0))
# ---------------------------------------------------------------------
# Map features
self.map_points = [] # Landmarks or keypoints in map
self.map_duration = 0.0 # Map duration in [s]
self.map_difficulty = 0.0 # Map difficulty [0.0-5.0]
self.map_distance = 0.0 # Map distance in [m]
self.way_points = {} # List of waypoints in the path planning routine
self._in_execution = False
# Read routines from the yaml file in the configs folder
self.routines = read_yaml_file(
CONF_PATH="/workspace/planner/configs",
FILE_NAME="routines.yaml",
)
# ---------------------------------------------------------------------
# Subscribers
self.sub_start_routine = self.create_subscription(
msg_type=Int32,
topic="/graphics/start_routine",
callback=self.cb_start_routine,
qos_profile=qos_profile_sensor_data,
callback_group=self.callback_group,
)
self.kiwibot_state = Kiwibot()
self.sub_kiwibot_stat = self.create_subscription(
msg_type=Kiwibot,
topic="/kiwibot/status",
callback=self.cb_kiwibot_status,
qos_profile=qos_profile_sensor_data,
callback_group=self.callback_group,
)
# ---------------------------------------------------------------------
# Publishers
self.pub_path_planner = self.create_publisher(
msg_type=planner_msg,
topic="/path_planner/msg",
qos_profile=qos_profile_sensor_data,
callback_group=self.callback_group,
)
self.pub_speaker = self.create_publisher(
msg_type=Int8,
topic="/device/speaker/command",
qos_profile=qos_profile_sensor_data,
callback_group=self.callback_group,
)
# ---------------------------------------------------------------------
# Services
# service client to turn the robot
self.cli_robot_turn = self.create_client(Turn, "/robot/turn")
# service client to move the robot
self.cli_robot_move = self.create_client(Move, "/robot/move")
try:
self.robot_turn_req = Turn.Request()
self.robot_move_req = Move.Request()
except Exception as e:
printlog(
msg="No services for robot actions, {}".format(e),
msg_type="ERROR",
)
def cb_start_routine(self, msg: Int32) -> None:
"""
Callback when a routine is started from visuals
Args:
msg: `Int32` number of routine to load waypoint from landmarks
Returns:
"""
try:
if self._in_execution:
printlog(msg="There's already a routine in execution",
msg_type="WARN")
return
self._in_execution = True
# Check that the routine in received exists in the routines list
if msg.data in self.routines.keys():
# -------------------------------------------------------
# Read the waypoint or landmarks for the specified route
self.way_points = self.read_keypoints(
land_marks_path="/workspace/planner/configs/key_points.csv",
key_Points=self.routines[msg.data],
)
# Publish routine for graphics components
self.pub_path_planner.publish(
planner_msg(
land_marks=[
LandMark(
neighbors=[],
id=idx,
x=int(way_point_coord[0]),
y=int(way_point_coord[1]),
) for idx, way_point_coord in enumerate(
self.way_points["coords"])
],
distance=self.map_distance,
duration=self.map_duration,
difficulty=self.map_difficulty,
))
# -------------------------------------------------------
# Get the robot in the initial position
printlog(
msg="setting the robot in origin",
msg_type="OKPURPLE",
)
self.robot_move_req.waypoints = [
Waypoint(
id=0,
x=int(self.way_points["coords"][0][0]),
y=int(self.way_points["coords"][0][1]),
t=0.0,
dt=0.0,
)
]
move_resp = self.cli_robot_move.call(self.robot_move_req)
# -------------------------------------------------------
# Execute planning process
self.pub_speaker.publish(Int8(data=2))
for idx, way_point in enumerate(
self.way_points["coords"][:-1]):
# -------------------------------------------------------
# Calculate the angle to turn the robot
dy = (self.way_points["coords"][idx][1] -
self.way_points["coords"][idx + 1][1])
dx = (self.way_points["coords"][idx + 1][0] -
self.way_points["coords"][idx][0])
ang = np.rad2deg(np.arctan2(dy, dx))
dang = ang - self.kiwibot_state.yaw
if abs(dang) > 180:
dang += 360
elif dang > 360:
dang -= 360
elif dang > 180:
dang -= 360
elif dang < -180:
dang += 360
if int(dang):
printlog(
msg=f"turning robot to reference {idx+1}",
msg_type="OKPURPLE",
)
# Generate the turning profile to get the robot aligned to the next landmark
self.robot_turn_req.turn_ref = [
TurnRef(
id=turn_reference["idx"],
yaw=turn_reference["a"],
t=turn_reference["t"],
dt=turn_reference["dt"],
) for turn_reference in self.get_profile_turn(
dst=dang,
time=self._TURN_TIME,
pt=self._TURN_ACELERATION_FC,
n=self._TURN_CRTL_POINTS,
)
]
move_resp = self.cli_robot_turn.call(
self.robot_turn_req)
# -------------------------------------------------------
printlog(
msg=f"moving robot to landmark {idx}",
msg_type="OKPURPLE",
)
# Generate the waypoints to the next landmark
seg_way_points = self.get_profile_route(
src=self.way_points["coords"][idx],
dst=self.way_points["coords"][idx + 1],
time=self.way_points["times"][idx],
pt=self._FORWARE_ACELERATION_FC,
n=self._FORWARE_CRTL_POINTS,
)
# Move the robot to the next landmark
self.robot_move_req.waypoints = [
Waypoint(
id=wp["idx"],
x=int(wp["pt"][0]),
y=int(wp["pt"][1]),
t=wp["t"],
dt=wp["dt"],
) for wp in seg_way_points
]
move_resp = self.cli_robot_move.call(self.robot_move_req)
# -------------------------------------------------------
if not self._in_execution:
printlog(
msg=f"routine {msg.data} execution has been stopped",
msg_type="WARN",
)
else:
printlog(
msg=f"routine {msg.data} has finished",
msg_type="OKGREEN",
)
# -------------------------------------------------------
self.pub_speaker.publish(Int8(data=3))
else:
printlog(
msg=f"routine {msg.data} does not exit",
msg_type="WARN",
)
return
except Exception as e:
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
printlog(
msg="{}, {}, {}, {}".format(e, exc_type, fname,
exc_tb.tb_lineno),
msg_type="ERROR",
)
self._in_execution = False
def run(self) -> None:
"""
Callback to update & draw window components
Args:
Returns:
"""
if self._win_background is None or self._kiwibot_img is None:
return
try:
while True:
win_img = self.draw_map()
if not self.msg_kiwibot.moving:
print_list_text(
win_img,
["press 1 to 9 to start a routine"],
origin=(
win_img.shape[1] - 550,
int(win_img.shape[0] * 0.95),
),
color=(0, 0, 255),
line_break=20,
thickness=1,
fontScale=0.8,
)
# Update the images dictionary in the callback action
cv2.imshow(self._win_name, win_img)
key = cv2.waitKey(self._win_time)
# No key
if key == -1:
continue
# Key1=1048633 & Key9=1048625
elif key >= 48 and key <= 57:
printlog(
msg=f"Code is broken here",
msg_type="WARN",
)
continue # remove this line
printlog(
msg=f"Routine {chr(key)} was sent to path planner node",
msg_type="INFO",
)
self.pub_start_routine.publish(Int32(data=int(chr(key))))
else:
printlog(
msg=f"No action for key {chr(key)} -> {key}",
msg_type="WARN",
)
except Exception as e:
exc_type, exc_obj, exc_tb = sys.exc_info()
fname = os.path.split(exc_tb.tb_frame.f_code.co_filename)[1]
printlog(
msg="{}, {}, {}, {}".format(e, exc_type, fname,
exc_tb.tb_lineno),
msg_type="ERROR",
)