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