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))