def ros_state_to_grpc(self, state):
grpc_state_msg = ros_pb2.RobotStatusUpdate()
grpc_state_msg.status_update = state.value #TODO I am not sure if this is valid
grpc_communication_msg = ros_pb2.RosToServerCommunication(
robot_name=self.robot_name, status_update=grpc_state_msg)
return grpc_communication_msg
def GenerateRequest():
yield ros_pb2.RosToServerCommunication(robot_name='client_test')
yield ros_pb2.RosToServerCommunication(raw_map=ros_pb2.RawMap(
resolution=1,
height=5,
width=5,
data=np.array([
0, 0, 0, 0, 0, 0, 101, 101, 101, 0, 0, 101, 0, 101, 0, 0, 101, 101,
101, 0, 0, 0, 0, 0, 0
],
dtype='uint8').tobytes(),
origin=ros_pb2.GpsCoordinates(longitude=10, latitude=20),
shift_x=10.0,
shift_y=20.0))
time.sleep(1)
yield ros_pb2.RosToServerCommunication(
robot_pose=ros_pb2.LocalMapPose(row=0, column=4, angle=45))
for update_request in iter(state_update_requests.get, None):
yield update_request
def main(argv):
with grpc.insecure_channel(f'localhost:{FLAGS.port}') as channel:
stub = ros_pb2_grpc.RosServiceStub(channel)
communication_generator = stub.Communicate(GenerateRequest())
try:
for communication in communication_generator:
print(communication)
if communication.HasField("navigation_request"):
state_update_requests.put(
ros_pb2.RosToServerCommunication(
status_update=ros_pb2.RobotStatusUpdate(
status_update=ros_pb2.RobotStatusUpdate.
NAVIGATING)))
state_update_requests.put(
ros_pb2.RosToServerCommunication(
status_update=ros_pb2.RobotStatusUpdate(
status_update=ros_pb2.RobotStatusUpdate.
NAVIGATING_AND_IDLE)))
except KeyError:
communication_generator.cancel()
def stub_callback(self):
"""Respond back to server
Args:
None
Yields:
RosToServerCommunication message with the robot name
then pops messages from the server communication_queue
"""
yield ros_pb2.RosToServerCommunication(robot_name=self.robot_name)
for communication in iter(self.robot_manager.communication_queue.get,
None):
communication = self.fill_communication_msg(communication)
yield communication
def ros_to_grpc_pose(self, pose):
x = (pose.pose.position.x)
y = (pose.pose.position.y)
orientation = [
pose.pose.orientation.x, pose.pose.orientation.y,
pose.pose.orientation.z, pose.pose.orientation.w
]
euler = tf.transformations.euler_from_quaternion(orientation)
yaw = euler[2]
angle = yaw
current_time = self.get_time_now()
grpc_pose = ros_pb2.RosToServerCommunication(
robot_pose=ros_pb2.LocalMapPose(
row=x, column=y, angle=angle, timestamp=current_time))
return grpc_pose
def map_callback(self, occupancy_grid):
"""Queue the map in the server main queue
Args:
None
Returns:
None
"""
metadata = occupancy_grid.info
map_raw_data = occupancy_grid.data
map_raw_data_encoded = self.encode(map_raw_data)
gps_coordinates_msg = ros_pb2.GpsCoordinates()
if self.gps_origin is not None:
gps_coordinates_msg.longitude = self.gps_origin.long
gps_coordinates_msg.latitude = self.gps_origin.lat
gps_coordinates_msg.longitude = 0
gps_coordinates_msg.latitude = 0
delta = 0
print metadata.origin.position.x
print metadata.origin.position.y
grpc_raw_map = ros_pb2.RosToServerCommunication(
raw_map=ros_pb2.RawMap(resolution=metadata.resolution,
height=metadata.height,
width=metadata.width,
shift_x=metadata.origin.position.x,
shift_y=metadata.origin.position.y,
data=map_raw_data_encoded,
origin_angle_shift=delta,
origin=gps_coordinates_msg))
self.communication_queue.put(grpc_raw_map)
rospy.loginfo("Sending map to server...")