Skip to content

Commit

Permalink
Merge branch 'main' into chore/debug/add_existence_prob_for_rviz
Browse files Browse the repository at this point in the history
  • Loading branch information
miursh authored Jan 8, 2024
2 parents f2c4536 + f4ea429 commit 515b355
Show file tree
Hide file tree
Showing 94 changed files with 3,476 additions and 2,485 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -52,7 +52,7 @@ struct NodeInterface
{ServiceLog::CLIENT_RESPONSE, "client exit"},
{ServiceLog::ERROR_UNREADY, "client unready"},
{ServiceLog::ERROR_TIMEOUT, "client timeout"}});
RCLCPP_INFO_STREAM(node->get_logger(), type_text.at(type) << ": " << name);
RCLCPP_DEBUG_STREAM(node->get_logger(), type_text.at(type) << ": " << name);

ServiceLog msg;
msg.stamp = node->now();
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,6 +17,7 @@

#include "autoware_auto_planning_msgs/msg/detail/trajectory__struct.hpp"
#include "autoware_auto_planning_msgs/msg/detail/trajectory_point__struct.hpp"
#include "std_msgs/msg/header.hpp"

#include <vector>

Expand All @@ -34,7 +35,8 @@ namespace motion_utils
* points larger than the capacity. (Tier IV)
*/
autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory);
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory,
const std_msgs::msg::Header & header = std_msgs::msg::Header{});

/**
* @brief Convert autoware_auto_planning_msgs::msg::Trajectory to
Expand Down
2 changes: 1 addition & 1 deletion common/motion_utils/src/resample/resample.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -486,7 +486,7 @@ autoware_auto_planning_msgs::msg::Path resamplePath(
autoware_auto_planning_msgs::msg::Path resampled_path;
resampled_path.header = input_path.header;
resampled_path.left_bound = input_path.left_bound;
resampled_path.right_bound = resampled_path.right_bound;
resampled_path.right_bound = input_path.right_bound;
resampled_path.points.resize(interpolated_pose.size());
for (size_t i = 0; i < resampled_path.points.size(); ++i) {
autoware_auto_planning_msgs::msg::PathPoint path_point;
Expand Down
4 changes: 3 additions & 1 deletion common/motion_utils/src/trajectory/conversion.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,9 +30,11 @@ namespace motion_utils
* points larger than the capacity. (Tier IV)
*/
autoware_auto_planning_msgs::msg::Trajectory convertToTrajectory(
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory)
const std::vector<autoware_auto_planning_msgs::msg::TrajectoryPoint> & trajectory,
const std_msgs::msg::Header & header)
{
autoware_auto_planning_msgs::msg::Trajectory output{};
output.header = header;
for (const auto & pt : trajectory) output.points.push_back(pt);
return output;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,20 +41,20 @@ class Client
const typename ServiceT::Request::SharedPtr & request,
const std::chrono::nanoseconds & timeout = std::chrono::seconds(2))
{
RCLCPP_INFO(logger_, "client request");
RCLCPP_DEBUG(logger_, "client request");

if (!client_->service_is_ready()) {
RCLCPP_INFO(logger_, "client available");
RCLCPP_DEBUG(logger_, "client available");
return {response_error("Internal service is not available."), nullptr};
}

auto future = client_->async_send_request(request);
if (future.wait_for(timeout) != std::future_status::ready) {
RCLCPP_INFO(logger_, "client timeout");
RCLCPP_DEBUG(logger_, "client timeout");
return {response_error("Internal service has timed out."), nullptr};
}

RCLCPP_INFO(logger_, "client response");
RCLCPP_DEBUG(logger_, "client response");
return {response_success(), future.get()};
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -263,9 +263,9 @@ void ManualController::onClickEnableButton()
{
auto req = std::make_shared<EngageSrv::Request>();
req->engage = true;
RCLCPP_INFO(raw_node_->get_logger(), "client request");
RCLCPP_DEBUG(raw_node_->get_logger(), "client request");
if (!client_engage_->service_is_ready()) {
RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable");
RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
return;
}
client_engage_->async_send_request(
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -38,6 +38,14 @@ Planning:
- node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance

behavior_path_planner_goal_planner:
- node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.goal_planner

behavior_path_planner_start_planner:
- node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.start_planner

behavior_path_avoidance_by_lane_change:
- node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
logger_name: lane_change.AVOIDANCE_BY_LANE_CHANGE
Expand Down
6 changes: 3 additions & 3 deletions common/tier4_state_rviz_plugin/src/autoware_state_panel.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -171,15 +171,15 @@ public Q_SLOTS: // NOLINT for Qt
{
auto req = std::make_shared<typename T::Request>();

RCLCPP_INFO(raw_node_->get_logger(), "client request");
RCLCPP_DEBUG(raw_node_->get_logger(), "client request");

if (!client->service_is_ready()) {
RCLCPP_INFO(raw_node_->get_logger(), "client is unavailable");
RCLCPP_DEBUG(raw_node_->get_logger(), "client is unavailable");
return;
}

client->async_send_request(req, [this](typename rclcpp::Client<T>::SharedFuture result) {
RCLCPP_INFO(
RCLCPP_DEBUG(
raw_node_->get_logger(), "Status: %d, %s", result.get()->status.code,
result.get()->status.message.c_str());
});
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -174,9 +174,6 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray::
case SteeringFactor::APPROACHING:
label->setText("APPROACHING");
break;
case SteeringFactor::TRYING:
label->setText("TRYING");
break;
case SteeringFactor::TURNING:
label->setText("TURNING");
break;
Expand Down
2 changes: 1 addition & 1 deletion common/traffic_light_utils/src/traffic_light_utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -46,7 +46,7 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float
signal.elements[0].shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
// the default value is -1, which means to not set confidence
if (confidence >= 0) {
if (confidence > 0) {
signal.elements[0].confidence = confidence;
}
}
Expand Down
23 changes: 23 additions & 0 deletions control/joy_controller/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,7 @@
| `steering_angle_velocity` | double | steering angle velocity for operation |
| `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) |
| `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) |
| `raw_control` | bool | skip input odometry if true |
| `velocity_gain` | double | ratio to calculate velocity by acceleration |
| `max_forward_velocity` | double | absolute max velocity to go forward |
| `max_backward_velocity` | double | absolute max velocity to go backward |
Expand Down Expand Up @@ -85,3 +86,25 @@
| Autoware Disengage ||
| Vehicle Engage ||
| Vehicle Disengage ||

## XBOX Joystick Key Map

| Action | Button |
| -------------------- | --------------------- |
| Acceleration | RT |
| Brake | LT |
| Steering | Left Stick Left Right |
| Shift up | Cursor Up |
| Shift down | Cursor Down |
| Shift Drive | Cursor Left |
| Shift Reverse | Cursor Right |
| Turn Signal Left | LB |
| Turn Signal Right | RB |
| Clear Turn Signal | A |
| Gate Mode | B |
| Emergency Stop | View |
| Clear Emergency Stop | Menu |
| Autoware Engage | X |
| Autoware Disengage | Y |
| Vehicle Engage | Left Stick Button |
| Vehicle Disengage | Right Stick Button |
1 change: 1 addition & 0 deletions control/joy_controller/config/joy_controller.param.yaml
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,7 @@
accel_sensitivity: 1.0
brake_sensitivity: 1.0
control_command:
raw_control: false
velocity_gain: 3.0
max_forward_velocity: 20.0
max_backward_velocity: 3.0
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -59,6 +59,7 @@ class AutowareJoyControllerNode : public rclcpp::Node
double brake_sensitivity_;

// ControlCommand Parameter
bool raw_control_;
double velocity_gain_;
double max_forward_velocity_;
double max_backward_velocity_;
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,81 @@
// Copyright 2023 The Autoware Contributors
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_
#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_

#include "joy_controller/joy_converter/joy_converter_base.hpp"

#include <algorithm>

namespace joy_controller
{
class XBOXJoyConverter : public JoyConverterBase
{
public:
explicit XBOXJoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {}

float accel() const { return std::max(0.0f, -((RT() - 1.0f) / 2.0f)); }

float brake() const { return std::max(0.0f, -((LT() - 1.0f) / 2.0f)); }

float steer() const { return LStickLeftRight(); }

bool shift_up() const { return CursorUpDown() == 1.0f; }
bool shift_down() const { return CursorUpDown() == -1.0f; }
bool shift_drive() const { return CursorLeftRight() == 1.0f; }
bool shift_reverse() const { return CursorLeftRight() == -1.0f; }

bool turn_signal_left() const { return LB(); }
bool turn_signal_right() const { return RB(); }
bool clear_turn_signal() const { return A(); }

bool gate_mode() const { return B(); }

bool emergency_stop() const { return ChangeView(); }
bool clear_emergency_stop() const { return Menu(); }

bool autoware_engage() const { return X(); }
bool autoware_disengage() const { return Y(); }

bool vehicle_engage() const { return LStickButton(); }
bool vehicle_disengage() const { return RStickButton(); }

private:
float LStickLeftRight() const { return j_.axes.at(0); }
float LStickUpDown() const { return j_.axes.at(1); }
float RStickLeftRight() const { return j_.axes.at(2); }
float RStickUpDown() const { return j_.axes.at(3); }
float RT() const { return j_.axes.at(4); }
float LT() const { return j_.axes.at(5); }
float CursorLeftRight() const { return j_.axes.at(6); }
float CursorUpDown() const { return j_.axes.at(7); }

bool A() const { return j_.buttons.at(0); }
bool B() const { return j_.buttons.at(1); }
bool X() const { return j_.buttons.at(3); }
bool Y() const { return j_.buttons.at(4); }
bool LB() const { return j_.buttons.at(6); }
bool RB() const { return j_.buttons.at(7); }
bool Menu() const { return j_.buttons.at(11); }
bool LStickButton() const { return j_.buttons.at(13); }
bool RStickButton() const { return j_.buttons.at(14); }
bool ChangeView() const { return j_.buttons.at(15); }
bool Xbox() const { return j_.buttons.at(16); }

const sensor_msgs::msg::Joy j_;
};
} // namespace joy_controller

#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_
2 changes: 1 addition & 1 deletion control/joy_controller/launch/joy_controller.launch.xml
Original file line number Diff line number Diff line change
@@ -1,5 +1,5 @@
<launch>
<arg name="joy_type" default="DS4" description="options: DS4, G29, P65"/>
<arg name="joy_type" default="DS4" description="options: DS4, G29, P65, XBOX"/>
<arg name="external_cmd_source" default="remote" description="options: local, remote"/>

<arg name="input_joy" default="/joy"/>
Expand Down
8 changes: 7 additions & 1 deletion control/joy_controller/schema/joy_controller.schema.json
Original file line number Diff line number Diff line change
Expand Up @@ -10,7 +10,7 @@
"type": "string",
"description": "Joy controller type",
"default": "DS4",
"enum": ["P65", "DS4", "G29"]
"enum": ["P65", "DS4", "G29", "XBOX"]
},
"update_rate": {
"type": "number",
Expand Down Expand Up @@ -55,6 +55,11 @@
"control_command": {
"type": "object",
"properties": {
"raw_control": {
"type": "boolean",
"description": "Whether to skip input odometry",
"default": false
},
"velocity_gain": {
"type": "number",
"description": "Ratio to calculate velocity by acceleration",
Expand All @@ -79,6 +84,7 @@
}
},
"required": [
"raw_control",
"velocity_gain",
"max_forward_velocity",
"max_backward_velocity",
Expand Down
18 changes: 13 additions & 5 deletions control/joy_controller/src/joy_controller/joy_controller_node.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#include "joy_controller/joy_converter/ds4_joy_converter.hpp"
#include "joy_controller/joy_converter/g29_joy_converter.hpp"
#include "joy_controller/joy_converter/p65_joy_converter.hpp"
#include "joy_controller/joy_converter/xbox_joy_converter.hpp"

#include <tier4_api_utils/tier4_api_utils.hpp>

Expand Down Expand Up @@ -154,6 +155,8 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt
joy_ = std::make_shared<const G29JoyConverter>(*msg);
} else if (joy_type_ == "DS4") {
joy_ = std::make_shared<const DS4JoyConverter>(*msg);
} else if (joy_type_ == "XBOX") {
joy_ = std::make_shared<const XBOXJoyConverter>(*msg);
} else {
joy_ = std::make_shared<const P65JoyConverter>(*msg);
}
Expand Down Expand Up @@ -217,7 +220,7 @@ bool AutowareJoyControllerNode::isDataReady()
}

// Twist
{
if (!raw_control_) {
if (!twist_) {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(),
Expand Down Expand Up @@ -458,6 +461,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions &
steering_angle_velocity_ = declare_parameter<double>("steering_angle_velocity");
accel_sensitivity_ = declare_parameter<double>("accel_sensitivity");
brake_sensitivity_ = declare_parameter<double>("brake_sensitivity");
raw_control_ = declare_parameter<bool>("control_command.raw_control");
velocity_gain_ = declare_parameter<double>("control_command.velocity_gain");
max_forward_velocity_ = declare_parameter<double>("control_command.max_forward_velocity");
max_backward_velocity_ = declare_parameter<double>("control_command.max_backward_velocity");
Expand All @@ -477,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions &
sub_joy_ = this->create_subscription<sensor_msgs::msg::Joy>(
"input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1),
subscriber_option);
sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
"input/odometry", 1,
std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1),
subscriber_option);
if (!raw_control_) {
sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
"input/odometry", 1,
std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1),
subscriber_option);
} else {
twist_ = std::make_shared<geometry_msgs::msg::TwistStamped>();
}

// Publisher
pub_control_command_ =
Expand Down
Loading

0 comments on commit 515b355

Please sign in to comment.