class Robot:
def __init__(self, id_r, weight, color, xd=0, yd=0):
self.id = id_r
self.weight = weight
self.color = color
self.speed_v = 0
self.speed_w = 0
self.speed_pub = None
self.pose_sub = None
self.pose = None
self.first_pose = None
self.mass = 0
self.xd = xd
self.yd = yd
self.neighbors = {}
self.weight_publisher = rospy.Publisher("/voronoi/robot_" + str(self.id) + "/weight", Float64, queue_size=1)
self.weight_publisher_str = rospy.Publisher("/voronoi/robot_" + str(self.id) + "/weight/str", String,
queue_size=1)
self.control = RobotControl()
def get_yaw(self):
(roll, pitch, yaw) = euler_from_quaternion(self.pose.orientation)
return yaw
def set_speed_publisher(self, topic):
self.speed_pub = rospy.Publisher(topic, Twist, queue_size=5)
self.init_robot_control()
def init_robot_control(self):
self.control.set_speed_publisher(self.speed_pub)
self.control.set_pose_updater(self.get_pose)
self.control.start()
def set_pose_subscriber(self, topic):
# type: (basestring) -> None
self.pose_sub = rospy.Subscriber(topic, Odometry, self.pose_callback)
def publish_speed(self, v, w):
# type: (float, float) -> None
if self.speed_pub is None:
raise ValueError("geometry_msgs/Twist publisher for robot " + self.id + " not initialized.")
else:
self.speed_v = v
self.speed_w = w
speed = Twist()
speed.linear.x = v
speed.angular.w = w
self.speed_pub.publish(speed)
def pose_callback(self, msg):
# type: (Odometry) -> None
if self.pose is None:
self.first_pose = msg.pose.pose
self.pose = msg.pose.pose
Util.publish_tf_transformation(self.first_pose, "robot_" + str(self.id) + "/odom", "/map")
def get_pose_array(self):
return [self.pose.position.x, self.pose.position.y]
def get_pose(self):
return self.pose
def __repr__(self):
rep = self.__dict__.copy()
del rep["pose"]
if rep["speed_pub"] is not None:
rep["speed_pub"] = "value set"
if rep["pose_sub"] is not None:
rep["pose_sub"] = "value set"
return json.dumps(rep)
def __str__(self):
return self.__repr__()
def set_pose(self, arr):
# type: (list) -> None
if self.pose is None:
self.pose = Pose()
self.pose.position.x = arr[0]
self.pose.position.y = arr[1]
def clear(self):
self.mass = 0
self.neighbors = {}
def get_kdel(self):
return np.matrix([[self.xd, self.yd], [self.xd, self.yd]])
