def lookup_transform_full( self, target_frame: str, target_time: Time, source_frame: str, source_time: Time, fixed_frame: str, timeout: Duration = Duration() ) -> TransformStamped: """ Get the transform from the source frame to the target frame using the advanced API. :param target_frame: Name of the frame to transform into. :param target_time: The time to transform to. (0 will get the latest) :param source_frame: Name of the input frame. :param source_time: The time at which source_frame will be evaluated. (0 will get the latest) :param fixed_frame: Name of the frame to consider constant in time. :param timeout: Time to wait for the target frame to become available. :return: The transform between the frames. """ goal = LookupTransform.Goal() goal.target_frame = target_frame goal.source_frame = source_frame goal.source_time = source_time.to_msg() goal.timeout = timeout.to_msg() goal.target_time = target_time.to_msg() goal.fixed_frame = fixed_frame goal.advanced = True return self.__process_goal(goal)
def convert_bicycle_to_odometry(self, state: bicycleModel.BicycleState, now: Time) -> Odometry: odom_msg = Odometry() odom_msg.header.frame_id = self.param_state_frame odom_msg.child_frame_id = self.param_odom_child_frame odom_msg.header.stamp = now.to_msg() odom_msg.pose.pose.position.x = state.x - np.cos( state.phi) * self.param_cog_to_rear_axle odom_msg.pose.pose.position.y = state.y - np.sin( state.phi) * self.param_cog_to_rear_axle odom_msg.pose.pose.position.z = 0.0 odom_msg.pose.pose.orientation.x = 0.0 odom_msg.pose.pose.orientation.y = 0.0 odom_msg.pose.pose.orientation.z = math.sin(state.phi / 2.0) odom_msg.pose.pose.orientation.w = math.cos(state.phi / 2.0) odom_msg.twist.twist.linear.x = state.v odom_msg.twist.twist.linear.y = 0.0 odom_msg.twist.twist.linear.z = 0.0 odom_msg.twist.twist.angular.x = 0.0 odom_msg.twist.twist.angular.y = 0.0 if (self._current_command is None): odom_msg.twist.twist.angular.z = 0.0 else: odom_msg.twist.twist.angular.z = state.v \ * math.tan(self._current_command.steering) / self.param_wheelbase return odom_msg
def convert_bicycle_to_vehicle_kinematic_state( self, state: bicycleModel.BicycleState, now: Time, prev_state: bicycleModel.BicycleState, dt_sec: float) -> VehicleKinematicState: state_msg = VehicleKinematicState() # Transform odom_H_cog to odom_H_base_link # TODO(s.merkli): Double check state_msg.state.x = state.x - np.cos( state.phi) * self.param_cog_to_rear_axle state_msg.state.y = state.y - np.sin( state.phi) * self.param_cog_to_rear_axle state_msg.state.heading = from_angle(state.phi) state_msg.state.longitudinal_velocity_mps = state.v state_msg.state.lateral_velocity_mps = 0.0 # not modeled in this state_msg.state.acceleration_mps2 = 0.0 # modeled as an input state_msg.state.heading_rate_rps = 0.0 # modeled as an input if (self._current_command is None): state_msg.state.front_wheel_angle_rad = 0.0 else: state_msg.state.front_wheel_angle_rad = self._current_command.steering state_msg.state.rear_wheel_angle_rad = 0.0 # not modeled in this state_msg.header.stamp = now.to_msg() state_msg.header.frame_id = self.param_state_frame if prev_state is not None: # for rear-wheel center state_msg.state.heading_rate_rps = state.v * math.tan(self._current_command.steering) \ / self.param_cog_to_rear_axle state_msg.state.acceleration_mps2 = (state.v - prev_state.v) / dt_sec return state_msg
def publish_trajectory(self, now: Time): kinematic_state = self.convert_bicycle_to_vehicle_kinematic_state( self._current_state, now, self._prev_state, self._simulator.step_time) # Initial trajectory, starting at the current state # TODO(s.merkli): Potentially use a fancier spoofer later init_trajectory_msg = self.create_curved_trajectory( kinematic_state, self.param_trajectory_length, self.param_trajectory_discretization_m, self.param_trajectory_speed_start, self.param_trajectory_speed_max, self.param_trajectory_speed_increments, self.param_trajectory_stopping_decel, self.param_heading_rate, self.param_heading_rate_max, self.param_heading_rate_increments, ) # Use simulated time init_trajectory_msg.header.stamp = now.to_msg() # Use ROS time # init_trajectory_msg.header.stamp = self.get_clock().now().to_msg() init_trajectory_msg.header.frame_id = self.param_trajectory_frame # Send first data to mpc # Both publishes trigger a control calculation but for the first, # there is missing information, so nothing happens self._publisher_trajectory.publish(init_trajectory_msg) # Make sure trajectory is sent first - TODO(s.merkli) can this be done # more systematically? self._traj_cache = init_trajectory_msg
def test_time_message_conversions_big_nanoseconds(self): time1 = Time(nanoseconds=1553575413247045598, clock_type=ClockType.ROS_TIME) builtins_msg = Builtins() builtins_msg.time_value = time1.to_msg() # Default clock type resulting from from_msg will be ROS time time2 = Time.from_msg(builtins_msg.time_value) assert isinstance(time2, Time) assert time1 == time2
def test_time_message_conversions(self): time1 = Time(nanoseconds=1, clock_type=ClockType.ROS_TIME) builtins_msg = Builtins() builtins_msg.time_value = time1.to_msg() # Default clock type resulting from from_msg will be ROS time time2 = Time.from_msg(builtins_msg.time_value) assert isinstance(time2, Time) assert time1 == time2 # Clock type can be specified if appropriate time3 = Time.from_msg(builtins_msg.time_value, clock_type=ClockType.SYSTEM_TIME) assert time3.clock_type == ClockType.SYSTEM_TIME
def convert_bicycle_to_transform(self, state: bicycleModel.BicycleState, now: Time) -> TFMessage: transform = TransformStamped() transform.header.frame_id = self.param_state_frame transform.header.stamp = now.to_msg() transform.child_frame_id = self.param_odom_child_frame transform.transform.translation.x = \ state.x - np.cos(state.phi) * self.param_cog_to_rear_axle transform.transform.translation.y = \ state.y - np.sin(state.phi) * self.param_cog_to_rear_axle transform.transform.translation.z = 0.0 transform.transform.rotation.x = 0.0 transform.transform.rotation.y = 0.0 transform.transform.rotation.z = math.sin(state.phi / 2.0) transform.transform.rotation.w = math.cos(state.phi / 2.0) return TFMessage(transforms=[transform])