Skip to content

Commit

Permalink
RT1-6683 add autoware prefix to package and namepace
Browse files Browse the repository at this point in the history
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
  • Loading branch information
zulfaqar-azmi-t4 committed Jun 11, 2024
1 parent 34307f1 commit 92d1125
Show file tree
Hide file tree
Showing 23 changed files with 119 additions and 118 deletions.
39 changes: 39 additions & 0 deletions control/autoware_pure_pursuit/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,39 @@
cmake_minimum_required(VERSION 3.14)
project(autoware_pure_pursuit)

find_package(autoware_cmake REQUIRED)
autoware_package()

find_package(Eigen3 REQUIRED)

include_directories(
SYSTEM
${EIGEN3_INCLUDE_DIRS}
)

# autoware_pure_pursuit_core
ament_auto_add_library(${PROJECT_NAME}_core SHARED
src/${PROJECT_NAME}_core/planning_utils.cpp
src/${PROJECT_NAME}_core/${PROJECT_NAME}.cpp
src/${PROJECT_NAME}_core/interpolate.cpp
)

# autoware_pure_pursuit
ament_auto_add_library(${PROJECT_NAME}_lateral_controller SHARED
src/${PROJECT_NAME}/${PROJECT_NAME}_lateral_controller.cpp
src/${PROJECT_NAME}/${PROJECT_NAME}_viz.cpp
)

target_link_libraries(${PROJECT_NAME}_lateral_controller
${PROJECT_NAME}_core
)

rclcpp_components_register_node(${PROJECT_NAME}_lateral_controller
PLUGIN "autoware::pure_pursuit::PurePursuitLateralController"
EXECUTABLE ${PROJECT_NAME}_lateral_controller_exe
)

ament_auto_package(INSTALL_TO_SHARE
launch
config
)
File renamed without changes.
Original file line number Diff line number Diff line change
Expand Up @@ -27,8 +27,8 @@
* limitations under the License.
*/

#ifndef PURE_PURSUIT__PURE_PURSUIT_HPP_
#define PURE_PURSUIT__PURE_PURSUIT_HPP_
#ifndef AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_
#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_

#include <rclcpp/rclcpp.hpp>

Expand All @@ -42,7 +42,7 @@
#include <Eigen/Core>
#include <Eigen/Geometry>

namespace pure_pursuit
namespace autoware::pure_pursuit
{
class PurePursuit
{
Expand Down Expand Up @@ -83,6 +83,6 @@ class PurePursuit
std::pair<bool, geometry_msgs::msg::Point> lerpNextTarget(int32_t next_wp_idx);
};

} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#endif // PURE_PURSUIT__PURE_PURSUIT_HPP_
#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -27,12 +27,12 @@
* limitations under the License.
*/

#ifndef PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_
#define PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_
#ifndef AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_
#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_

#include "autoware_trajectory_follower_base/lateral_controller_base.hpp"
#include "pure_pursuit/pure_pursuit.hpp"
#include "pure_pursuit/pure_pursuit_viz.hpp"
#include "autoware_pure_pursuit/autoware_pure_pursuit.hpp"
#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp"
#include "rclcpp/rclcpp.hpp"
#include "tf2_ros/buffer.h"
#include "tf2_ros/transform_listener.h"
Expand Down Expand Up @@ -60,7 +60,7 @@ using autoware_control_msgs::msg::Lateral;
using autoware_planning_msgs::msg::Trajectory;
using autoware_planning_msgs::msg::TrajectoryPoint;

namespace pure_pursuit
namespace autoware::pure_pursuit
{

struct PpOutput
Expand Down Expand Up @@ -174,6 +174,6 @@ class PurePursuitLateralController : public LateralControllerBase
mutable DebugData debug_data_;
};

} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#endif // PURE_PURSUIT__PURE_PURSUIT_LATERAL_CONTROLLER_HPP_
#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_LATERAL_CONTROLLER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -27,11 +27,11 @@
* limitations under the License.
*/

#ifndef PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_
#define PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_
#ifndef AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_
#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_

#include "pure_pursuit/pure_pursuit.hpp"
#include "pure_pursuit/pure_pursuit_viz.hpp"
#include "autoware_pure_pursuit/autoware_pure_pursuit.hpp"
#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp"

#include <rclcpp/rclcpp.hpp>
#include <tier4_autoware_utils/ros/polling_subscriber.hpp>
Expand All @@ -50,7 +50,7 @@

#include <memory>

namespace pure_pursuit
namespace autoware::pure_pursuit
{
struct Param
{
Expand Down Expand Up @@ -124,6 +124,6 @@ class PurePursuitNode : public rclcpp::Node
mutable DebugData debug_data_;
};

} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#endif // PURE_PURSUIT__PURE_PURSUIT_NODE_HPP_
#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_NODE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -27,21 +27,21 @@
* limitations under the License.
*/

#ifndef PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_
#define PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_
#ifndef AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_
#define AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_

#include <geometry_msgs/msg/pose_stamped.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <visualization_msgs/msg/marker_array.hpp>

#include <vector>
namespace pure_pursuit
namespace autoware::pure_pursuit
{
visualization_msgs::msg::Marker createNextTargetMarker(
const geometry_msgs::msg::Point & next_target);

visualization_msgs::msg::Marker createTrajectoryCircleMarker(
const geometry_msgs::msg::Point & target, const geometry_msgs::msg::Pose & current_pose);
} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#endif // PURE_PURSUIT__PURE_PURSUIT_VIZ_HPP_
#endif // AUTOWARE_PURE_PURSUIT__AUTOWARE_PURE_PURSUIT_VIZ_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,14 +12,14 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PURE_PURSUIT__UTIL__INTERPOLATE_HPP_
#define PURE_PURSUIT__UTIL__INTERPOLATE_HPP_
#ifndef AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_
#define AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_

#include <cmath>
#include <iostream>
#include <vector>

namespace pure_pursuit
namespace autoware::pure_pursuit
{
class LinearInterpolate
{
Expand Down Expand Up @@ -50,6 +50,6 @@ class SplineInterpolate
const std::vector<double> & return_index, std::vector<double> & return_value);
void getValueVector(const std::vector<double> & s_v, std::vector<double> & value_v);
};
} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#endif // PURE_PURSUIT__UTIL__INTERPOLATE_HPP_
#endif // AUTOWARE_PURE_PURSUIT__UTIL__INTERPOLATE_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,16 +12,16 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_
#define PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_
#ifndef AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_
#define AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_

#include <rclcpp/rclcpp.hpp>

#include <visualization_msgs/msg/marker_array.hpp>

#include <string>

namespace pure_pursuit
namespace autoware::pure_pursuit
{
inline geometry_msgs::msg::Point createMarkerPosition(double x, double y, double z)
{
Expand Down Expand Up @@ -102,6 +102,6 @@ inline void appendMarkerArray(
marker_array->markers.push_back(marker);
}
}
} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#endif // PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_
#endif // AUTOWARE_PURE_PURSUIT__UTIL__MARKER_HELPER_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_
#define PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_
#ifndef AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_
#define AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_

#define EIGEN_MPL2_ONLY

Expand Down Expand Up @@ -45,7 +45,7 @@

#define PLANNING_UTILS_LOGGER "planning_utils"

namespace pure_pursuit
namespace autoware::pure_pursuit
{
namespace planning_utils
{
Expand Down Expand Up @@ -139,6 +139,6 @@ geometry_msgs::msg::Point transformToRelativeCoordinate2D(
geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double _yaw);

} // namespace planning_utils
} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#endif // PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_
#endif // AUTOWARE_PURE_PURSUIT__UTIL__PLANNING_UTILS_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,8 +12,8 @@
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef PURE_PURSUIT__UTIL__TF_UTILS_HPP_
#define PURE_PURSUIT__UTIL__TF_UTILS_HPP_
#ifndef AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_
#define AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_

#include <rclcpp/rclcpp.hpp>

Expand All @@ -31,7 +31,7 @@

#define TF_UTILS_LOGGER "tf_utils"

namespace pure_pursuit
namespace autoware::pure_pursuit
{
namespace tf_utils
{
Expand Down Expand Up @@ -92,6 +92,6 @@ inline boost::optional<geometry_msgs::msg::PoseStamped> getCurrentPose(
}

} // namespace tf_utils
} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#endif // PURE_PURSUIT__UTIL__TF_UTILS_HPP_
#endif // AUTOWARE_PURE_PURSUIT__UTIL__TF_UTILS_HPP_
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>pure_pursuit</name>
<name>autoware_pure_pursuit</name>
<version>0.1.0</version>
<description>The pure_pursuit package</description>
<maintainer email="takamasa.horibe@tier4.jp">Takamasa Horibe</maintainer>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,11 @@
* limitations under the License.
*/

#include "pure_pursuit/pure_pursuit_lateral_controller.hpp"
#include "autoware_pure_pursuit/autoware_pure_pursuit_lateral_controller.hpp"

#include "pure_pursuit/pure_pursuit_viz.hpp"
#include "pure_pursuit/util/planning_utils.hpp"
#include "pure_pursuit/util/tf_utils.hpp"
#include "autoware_pure_pursuit/autoware_pure_pursuit_viz.hpp"
#include "autoware_pure_pursuit/util/planning_utils.hpp"
#include "autoware_pure_pursuit/util/tf_utils.hpp"

#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>

Expand All @@ -54,7 +54,7 @@ enum TYPE {
};
} // namespace

namespace pure_pursuit
namespace autoware::pure_pursuit
{
PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
: clock_(node.get_clock()),
Expand Down Expand Up @@ -261,7 +261,7 @@ void PurePursuitLateralController::averageFilterTrajectory(
p.heading_rate_rps = tmp.heading_rate_rps / count;
p.lateral_velocity_mps = tmp.lateral_velocity_mps / count;
p.rear_wheel_angle_rad = tmp.rear_wheel_angle_rad / count;
p.pose.orientation = pure_pursuit::planning_utils::getQuaternionFromYaw(yaw / count);
p.pose.orientation = autoware::pure_pursuit::planning_utils::getQuaternionFromYaw(yaw / count);
}
trajectory_resampled_ = std::make_shared<Trajectory>(filtered_trajectory);
}
Expand Down Expand Up @@ -487,4 +487,4 @@ boost::optional<PpOutput> PurePursuitLateralController::calcTargetCurvature(

return output;
}
} // namespace pure_pursuit
} // namespace autoware::pure_pursuit
Original file line number Diff line number Diff line change
Expand Up @@ -28,11 +28,11 @@
* limitations under the License.
*/

#include "pure_pursuit/pure_pursuit_node.hpp"
#include "autoware_pure_pursuit/autoware_pure_pursuit_node.hpp"

#include "pure_pursuit/pure_pursuit_viz.hpp"
#include "pure_pursuit/util/planning_utils.hpp"
#include "pure_pursuit/util/tf_utils.hpp"
#include "autoware_pure_pursuit/pure_pursuit_viz.hpp"
#include "autoware_pure_pursuit/util/planning_utils.hpp"
#include "autoware_pure_pursuit/util/tf_utils.hpp"

#include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>

Expand All @@ -51,7 +51,7 @@ double calcLookaheadDistance(

} // namespace

namespace pure_pursuit
namespace autoware::pure_pursuit
{
PurePursuitNode::PurePursuitNode(const rclcpp::NodeOptions & node_options)
: Node("pure_pursuit", node_options), tf_buffer_(this->get_clock()), tf_listener_(tf_buffer_)
Expand Down Expand Up @@ -208,7 +208,7 @@ boost::optional<autoware_planning_msgs::msg::TrajectoryPoint> PurePursuitNode::c

return trajectory_->points.at(closest_idx_result.second);
}
} // namespace pure_pursuit
} // namespace autoware::pure_pursuit

#include <rclcpp_components/register_node_macro.hpp>
RCLCPP_COMPONENTS_REGISTER_NODE(pure_pursuit::PurePursuitNode)
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::pure_pursuit::PurePursuitNode)
Loading

0 comments on commit 92d1125

Please sign in to comment.