def __parsed__(self, context): self.arguments = Argumentable().from_list(self.arguments) # Apply context to values self.points = self.__contextualized_value(str(self.points), int) for key in self.arguments.keys(): self.arguments.set(key, self.__contextualized_value(self.arguments.get(key)))
def compute_action_point(self, args: Argumentable,
robot_pos: Pose2D) -> ActionPoint:
res = ActionPoint()
res.start = robot_pos
res.end = Pose2D()
res.end.x = args.get("x", float, 0)
res.end.y = args.get("y", float, 0)
res.end.theta = args.get("angle", float, 0)
return res
def test_frame_output(self):
val = 27
poll_rate = rospy.Rate(100)
# Wait for can_interface to be ready
nh_handler = NodeStatusHandler()
nh_handler.require("can_interface", "interface")
nh_handler.set_wait_callback(self.can_interface_up)
nh_handler.wait_for_nodes(3)
start = time.time()
while not self.interface_up and time.time() - start < 2:
poll_rate.sleep()
self.assertTrue(self.interface_up, "can interface is up")
# Test all frames
for frame_name in self.frames.by_name:
if frame_name == "pong":
continue
frame = self.frames.by_name[frame_name]
# Prepare data to send
can_req = can.Message(
data=bytearray([frame.id] + [(val + 7 * i) % 256
for i in range(7)]),
arbitration_id=self.devices.by_name[frame.dest],
dlc=frame.size(),
is_extended_id=False)
val += 78
# Send data
self.bus.send(can_req)
# Wait for answer
start = time.time()
while time.time() - start < 1.1 and not rospy.is_shutdown()\
and len(self.messages) == 0:
poll_rate.sleep()
self.assertLess(
time.time() - start, 1,
"{} transmitted in less than a second".format(frame_name))
message = self.messages.pop()
initial = frame.extract_frame_data(can_req)
received = Argumentable().from_list(message.params)
self.assertEqual(message.type, frame.name, "right name")
self.assertEqual(initial.to_list(), received.to_list(),
"received same data")
def can_to_ros(self, frame: Message, values: Argumentable): """ Retrieve param data from frame data with binary operations and put in in the given argumentable """ value = frame.data[self.byte_start + self.size - 1] for index in range(self.size - 1): value = value | \ frame.data[self.byte_start + index] << (self.size - index - 1) * 8 values.set(self.name, int(value))
def on_ros_message(self, message):
"""
Handle message from ROS and build a frame from it
"""
rospy.logdebug("received frame to send of type {}".format(message.type))
# Get message params as argumentable
values = Argumentable().from_list(message.params)
frame_type = self.frames.by_name[message.type]
# Prepare data array and set frame type
data = frame_type.get_frame_data(values)
if data is not None:
# Setup output frame
frame: can.Message = can.Message(
timestamp=time.time(),
is_remote_frame=False,
is_error_frame=False,
is_extended_id=False,
dlc=frame_type.size(),
arbitration_id=self.devices.by_name[frame_type.dest],
data=data
)
# Send frame to ros
self.bus.send(frame)
def on_action_returns(self, state, result):
"""
Current action done
"""
self.current_action.state = result.state
# Declare points
if result.state == ActionStatus.DONE:
parent = self.current_action
while parent is not None and parent.state == ActionStatus.DONE:
self.points += parent.points
parent = parent.parent
frame = CanData()
frame.type = "update_screen"
frame.params = Argumentable({
"points":
self.points,
"status":
4 if self.root_action.state == ActionStatus.DONE else 2
}).to_list()
self.can_out.publish(frame)
self.next_action()
def send_can(self, type, params: Dict):
"""
Handle message from ROS and build a frame from it
"""
# Get message params as argumentable
values = Argumentable(params)
frame_type = self.frames.by_name[type]
# Prepare data array and set frame type
data = frame_type.get_frame_data(values)
if data is not None:
# Setup output frame
frame: can.Message = can.Message(
timestamp=time.time(),
is_remote_frame=False,
is_error_frame=False,
is_extended_id=False,
dlc=frame_type.size(),
arbitration_id=self.devices.by_name[frame_type.dest],
data=data)
# Send frame to ros
self.bus.send(frame)
def extract_frame_data(self, frame: 'can.Message') -> Argumentable:
"""
Extract data from frame data, and return an argumentable
having all the properties
"""
data = Argumentable()
for param in self.params:
param.can_to_ros(frame, data)
return data
def _compute_action_point(
self,
request: ComputeActionPointRequest) -> ComputeActionPointResponse:
# Extract data from request and call performer's function
result: ActionPoint = self.compute_action_point(
Argumentable().from_list(request.args), request.robot_pos)
response = ComputeActionPointResponse()
response.action_point = result
return response
def main():
interface = input("interface [can1] : ")
if interface == "":
interface = "can1"
can.rc['interface'] = 'socketcan_ctypes'
bus = can.Bus(interface)
devices = DeviceList.parse_file("can/devices.xml", "interface_description")
frames = FrameList.parse_file("can/frames.xml", "interface_description", context={"devices": devices})
while True:
frame_name = input("can frame to send : ")
while frame_name not in frames.by_name:
print("available frames :\n\t{}".format("\n\t".join(frames.by_name.keys())))
frame_name = input("can frame to send : ")
frame = frames.by_name[frame_name]
values = Argumentable()
for param in frame.params:
val = None
while val is None:
try:
val = int(input("value for {} : ".format(param.name)))
except ValueError:
print("value must be int")
values.set(param.name, val)
print("preparing frame")
message = can.Message(
data=frame.get_frame_data(values),
arbitration_id=devices.by_name[frame.dest],
is_extended_id=False,
dlc=frame.size()
)
print("sending frame")
bus.send(message)
def ros_to_can(self, data_array: List[int], values: Argumentable):
"""
Apply current parameter into data array from given values
"""
if not values.has(self.name):
raise MissingParameterException(self.name)
value = values.get(self.name, int)
if value < 0 or value > 255 ** self.size:
rospy.logerr("unable to encode {} value {}".format(self.name, value))
return
if self.size == 1:
data_array[self.byte_start] = value
elif self.size == 2:
data_array[self.byte_start] = value >> 8
data_array[self.byte_start + 1] = value & 0x00FF
else:
raise Exception("size not handled yet, go back to coding")
def test_frames_input(self):
val = 21
for frame_name in self.frames.by_name:
if frame_name == "pong":
continue
frame = self.frames.by_name[frame_name]
# Prepare data to send
req = CanData()
req.type = frame.name
# Put all values in message
initial = Argumentable()
for param in frame.params:
if param.size == 2:
initial.set(param.name, (val * 267 + 25) % (256 * 256))
else:
initial.set(param.name, val)
val = (val + 12) % 256
req.params = initial.to_list()
self.output_pub.publish(req)
response = self.bus.recv(timeout=1)
self.assertNotEqual(response, None, "response received")
self.assertEqual(response.arbitration_id,
self.devices.by_name[frame.dest],
"right frame received")
self.assertEqual(response.data[0], frame.id,
"right frame received")
# Check received data
received = frame.extract_frame_data(response)
# Assert equal to initial
for param in frame.params:
self.assertEqual(initial.get(param), received.get(param),
"received data intact")
def on_ros_message(self, message):
"""
Handle message from ROS and build a frame from it
"""
rospy.logdebug("received frame to send of type {}".format(
message.type))
# Get message params as argumentable
values = Argumentable().from_list(message.params)
frame_type = self.frames.by_name[message.type]
# Prepare data array and set frame type
data = frame_type.get_frame_data(values)
if data is not None:
for bus in self.serials:
if bus.enabled:
bus.serial.write(bytes(data[:frame_type.size()]))
bus.serial.flush()
def start(self, args: Argumentable):
time.sleep(args.get("duration", float))
# Mark action as done after waiting
self.returns(ActionStatus.DONE)
class Action:
action_id = 0
def __init__(self):
# Object id
self.action_id = Action.action_id
Action.action_id += 1
self.name: str = ""
self.native = False
self.points = 0
self.arguments: Argumentable = []
self.requirements: List[ObjectRequirement] = []
self.parent: Union['ActionGroup', None] = None
self.__action_point: Union[ActionPoint, None] = None
self.__action_point_origin: Union[ActionPoint, None] = None
self.__state = ActionStatus.IDLE
def set_side(self, side: int):
# Set side as argument
self.arguments.set("_side", side)
def __before_children__(self, context):
# Get parent arguments
if context.parent is not None:
self.arguments.extend(context.parent.arguments)
def __parsed__(self, context):
self.arguments = Argumentable().from_list(self.arguments)
# Apply context to values
self.points = self.__contextualized_value(str(self.points), int)
for key in self.arguments.keys():
self.arguments.set(key, self.__contextualized_value(self.arguments.get(key)))
def __contextualized_value(self, value: str, cast: Type = str):
# TODO check
if len(value) > 0 and value[0] == "@":
if self.arguments.has(value[1:]):
return cast(self.arguments.get(value[1:]))
else:
return cast()
else:
return cast(value) if len(value) > 0 else cast()
@property
def state(self) -> int:
return self.__state
@state.setter
def state(self, state: int):
self.__state = state
# If not resuming, we propagate to parent
if state != ActionStatus.IDLE and self.parent != None:
self.parent.state = state
def total_points(self):
return self.points
def action_point(self, origin: Pose2D) -> ActionPoint:
'''
Compute the initial and final point of the action.
Calls associated performer service to let it compute the point based on args
'''
# Check if previously saved for this origin
if self.__action_point == None or self.__action_point_origin != origin:
point_service = rospy.ServiceProxy(
get_action_point_service(self.name),
ComputeActionPoint
)
# Wait 2s max for service
point_service.wait_for_service(2)
try:
self.__action_point = point_service(origin, self.arguments.to_list()).action_point
self.__action_point_origin = origin
except rospy.ServiceException:
rospy.logerr("unable to process action point for {}".format(self.name))
return self.__action_point
def travel_distance(self, origin: Pose2D) -> float:
'''Compute the estimated distance to travel before the robot reach the end of the action'''
point = self.action_point(origin)
# Return euclidian distance
return math.sqrt(
pow(point.start.x - origin.x, 2) +
pow(point.start.y - origin.y, 2)
) + math.sqrt(
pow(point.start.x - point.end.x, 2) +
pow(point.start.y - point.end.y, 2)
)
def priority(self, origin: Pose2D) -> float:
points = self.total_points()
distance = self.travel_distance(origin)
if distance == 0: # handle divide by 0
return math.inf
else:
return points * points / distance
def get_optimal(self, robot_pos: Pose2D) -> ActionChoice:
"""
Returns the optimal choice of native action from this action
"""
# In a simple action, only two cases : action unavailable or not
if self.state != ActionStatus.IDLE:
return ActionChoice(None, -1)
else:
return ActionChoice(self, self.priority(robot_pos))
def color(self, text):
if self.state == ActionStatus.PAUSED:
# Yellow
return '\033[33m' + text + '\033[0m'
elif self.state == ActionStatus.DONE:
# Green
return '\033[32m' + text + '\033[0m'
elif self.state == ActionStatus.IDLE:
# Grey
return '\033[90m' + text + '\033[0m'
else:
# Red
return '\033[31m' + text + '\033[0m'
def __str__(self):
return "{} points={} {{{}}}".format(
self.color("[{}@{}]".format(self.name, self.action_id)), self.total_points(), self.arguments.__str__()
# self.color("[{}@{}]".format(self.name, self.action_id)), self.total_points(), ""
)
def start(self, args: Argumentable): msg = CanData() msg.type = "fetch_puck" msg.params = args.to_list() self.can_out.publish(msg)
def start(self, args: Argumentable): msg = CanData() msg.type = "set_dock_height" msg.params = args.to_list() self.can_out.publish(msg)
def _on_goal(self):
if self._action_server.is_new_goal_available():
goal: PerformGoal = self._action_server.accept_new_goal()
# run action with given args
self.start(Argumentable().from_list(goal.arguments))
