Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] committed Sep 4, 2024
1 parent b0cf6e9 commit 08e27bb
Show file tree
Hide file tree
Showing 4 changed files with 48 additions and 23 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,9 @@ DummyOperationModePublisher::DummyOperationModePublisher(const rclcpp::NodeOptio
// Publisher
pub_operation_mode_state_ = create_publisher<autoware_adapi_v1_msgs::msg::OperationModeState>(
"~/output/operation_mode_state", 10);
pub_operation_mode_state_adapi_ = create_publisher<autoware_adapi_v1_msgs::msg::OperationModeState>(
"~/output/operation_mode_state_adapi", 10);
pub_operation_mode_state_adapi_ =
create_publisher<autoware_adapi_v1_msgs::msg::OperationModeState>(
"~/output/operation_mode_state_adapi", 10);

// Service

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -336,7 +336,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
RCLCPP_INFO(get_logger(), "external velocity limit pointer exist!!!");
// sender
external_velocity_limit_.sender = external_velocity_limit_ptr_->sender;
RCLCPP_INFO(get_logger(), "external velocity limit sender: %s", external_velocity_limit_.sender.c_str());
RCLCPP_INFO(
get_logger(), "external velocity limit sender: %s", external_velocity_limit_.sender.c_str());

// on the first time, apply directly
if (prev_output_.empty() || !current_closest_point_from_prev_output_) {
Expand Down Expand Up @@ -378,7 +379,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
external_velocity_limit_ptr_->use_constraints ? cstr.max_jerk : smoother_->getMaxJerk();
const auto j_min =
external_velocity_limit_ptr_->use_constraints ? cstr.min_jerk : smoother_->getMinJerk();
RCLCPP_INFO(get_logger(), "constraints: a_min = %f, j_max = %f, j_min = %f", a_min, j_max, j_min);
RCLCPP_INFO(
get_logger(), "constraints: a_min = %f, j_max = %f, j_min = %f", a_min, j_max, j_min);

// If the closest acceleration is positive, velocity will increase
// until the acceleration becomes zero
Expand All @@ -388,7 +390,8 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
} else {
max_velocity_with_deceleration_ = v0;
}
RCLCPP_INFO(get_logger(), "max_velocity_with_deceleration = %f", max_velocity_with_deceleration_);
RCLCPP_INFO(
get_logger(), "max_velocity_with_deceleration = %f", max_velocity_with_deceleration_);

if (external_velocity_limit_ptr_->max_velocity < max_velocity_with_deceleration_) {
// TODO(mkuri) If v0 < external_velocity_limit_ptr_->max_velocity <
Expand All @@ -409,11 +412,13 @@ void MotionVelocitySmootherNode::calcExternalVelocityLimit()
RCLCPP_WARN(get_logger(), "Stop distance calculation failed!");
}
external_velocity_limit_.dist = stop_dist + margin;
RCLCPP_INFO(get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
RCLCPP_INFO(
get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
} else {
max_velocity_with_deceleration_ = external_velocity_limit_ptr_->max_velocity;
external_velocity_limit_.dist = 0.0;
RCLCPP_INFO(get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
RCLCPP_INFO(
get_logger(), "external velocity limit dist = %f", external_velocity_limit_.dist);
}
}
}
Expand Down Expand Up @@ -910,7 +915,8 @@ void MotionVelocitySmootherNode::applyExternalVelocityLimit(TrajectoryPoints & t
// apply external velocity limit from the inserted point
trajectory_utils::applyMaximumVelocityLimit(
*inserted_index, traj.size(), external_velocity_limit_.velocity, traj);
RCLCPP_INFO(get_logger(), "apply external velocity limit: dist = %f, vel = %f",
RCLCPP_INFO(
get_logger(), "apply external velocity limit: dist = %f, vel = %f",
external_velocity_limit_.dist, external_velocity_limit_.velocity);

// create virtual wall
Expand Down
5 changes: 2 additions & 3 deletions system/leader_election_converter/package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -11,13 +11,12 @@
<buildtool_depend>autoware_cmake</buildtool_depend>

<depend>autoware_adapi_v1_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_auto_vehicle_msgs</depend>
<depend>geometry_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tier4_system_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>geometry_msgs</depend>


<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>
Expand Down
43 changes: 31 additions & 12 deletions system/leader_election_converter/script/relay_to_sub.py
Original file line number Diff line number Diff line change
@@ -1,25 +1,43 @@
#!/usr/bin/env python3

import rclpy
from rclpy.node import Node
from autoware_adapi_v1_msgs.msg import OperationModeState
from autoware_auto_planning_msgs.msg import Trajectory
from geometry_msgs.msg import PoseWithCovarianceStamped
import rclpy
from rclpy.node import Node
from tier4_system_msgs.msg import OperationModeAvailability

class RemapNode(Node):

class RemapNode(Node):
def __init__(self):
super().__init__('remap_node')
super().__init__("remap_node")

self.create_subscription(OperationModeAvailability, '/system/operation_mode/availability', self.operation_mode_callback, 10)
self.create_subscription(OperationModeState, '/system/operation_mode/state', self.operation_mode_state_callback, 10)
self.sub_trajectory = self.create_subscription(Trajectory, '/planning/scenario_planning/trajectory', self.trajectory_callback, 10)
self.sub_pose_with_covariance = self.create_subscription(PoseWithCovarianceStamped, '/localization/pose_with_covariance', self.pose_callback, 10)
self.create_subscription(
OperationModeAvailability,
"/system/operation_mode/availability",
self.operation_mode_callback,
10,
)
self.create_subscription(
OperationModeState,
"/system/operation_mode/state",
self.operation_mode_state_callback,
10,
)
self.sub_trajectory = self.create_subscription(
Trajectory, "/planning/scenario_planning/trajectory", self.trajectory_callback, 10
)
self.sub_pose_with_covariance = self.create_subscription(
PoseWithCovarianceStamped, "/localization/pose_with_covariance", self.pose_callback, 10
)
# self.sub_initialpose3d = self.create_subscription(PoseWithCovarianceStamped, '/initialpose3d', self.initialpose_callback, 10)

self.pub_trajectory = self.create_publisher(Trajectory, '/to_sub/planning/scenario_planning/trajectory', 10)
self.pub_pose_with_covariance = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/localization/pose_with_covariance', 10)
self.pub_trajectory = self.create_publisher(
Trajectory, "/to_sub/planning/scenario_planning/trajectory", 10
)
self.pub_pose_with_covariance = self.create_publisher(
PoseWithCovarianceStamped, "/to_sub/localization/pose_with_covariance", 10
)
# self.pub_initialpose3d = self.create_publisher(PoseWithCovarianceStamped, '/to_sub/initialpose3d', 10)

self.autonomous_mode = False
Expand All @@ -36,7 +54,6 @@ def operation_mode_state_callback(self, msg):
self.operation_mode_autonomous_state == True
else:
self.operation_mode_autonomous_state == False


def trajectory_callback(self, msg):
if self.autonomous_mode or self.operation_mode_autonomous_state == False:
Expand All @@ -50,12 +67,14 @@ def pose_callback(self, msg):
# if self.autonomous_mode:
# self.pub_initialpose3d.publish(msg)


def main(args=None):
rclpy.init(args=args)
node = RemapNode()
rclpy.spin(node)
node.destroy_node()
rclpy.shutdown()

if __name__ == '__main__':

if __name__ == "__main__":
main()

0 comments on commit 08e27bb

Please sign in to comment.