From dd94314e5be462693875f04023f22b691d759830 Mon Sep 17 00:00:00 2001 From: MaxPacheco02 Date: Fri, 25 Oct 2024 13:16:17 -0600 Subject: [PATCH 1/4] init commit --- workspace/src/sdv_control/CMakeLists.txt | 9 + .../src/model/sdv_dynamic_node.cpp | 175 ++++++++++++++++++ 2 files changed, 184 insertions(+) create mode 100644 workspace/src/sdv_control/src/model/sdv_dynamic_node.cpp diff --git a/workspace/src/sdv_control/CMakeLists.txt b/workspace/src/sdv_control/CMakeLists.txt index c1d9bcde..1358a879 100644 --- a/workspace/src/sdv_control/CMakeLists.txt +++ b/workspace/src/sdv_control/CMakeLists.txt @@ -64,6 +64,14 @@ target_compile_features(sdv_kinematic_sim PUBLIC c_std_99 cxx_std_17) ament_target_dependencies(sdv_kinematic_sim ${DEPENDENCIES}) target_link_libraries(sdv_kinematic_sim vanttec_controllers) +add_executable(sdv_dynamic_node src/model/sdv_dynamic_node.cpp) +target_include_directories(sdv_dynamic_node PUBLIC + $ + $ libs/vanttec_controllers) +target_compile_features(sdv_dynamic_node PUBLIC c_std_99 cxx_std_17) +ament_target_dependencies(sdv_dynamic_node ${DEPENDENCIES}) +target_link_libraries(sdv_dynamic_node vanttec_controllers) + add_executable(velocity_regulator_node src/control/regulators/first_order/velocity_regulator_node.cpp) target_include_directories(velocity_regulator_node PUBLIC $ @@ -77,6 +85,7 @@ install(TARGETS stanley_controller_node sdv_kinematic_sim velocity_regulator_node + sdv_dynamic_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY diff --git a/workspace/src/sdv_control/src/model/sdv_dynamic_node.cpp b/workspace/src/sdv_control/src/model/sdv_dynamic_node.cpp new file mode 100644 index 00000000..19590a80 --- /dev/null +++ b/workspace/src/sdv_control/src/model/sdv_dynamic_node.cpp @@ -0,0 +1,175 @@ +#include + +#include +#include +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/pose2_d.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "std_msgs/msg/u_int8.hpp" +#include "nav_msgs/msg/path.hpp" +#include "rclcpp/rclcpp.hpp" +#include "std_msgs/msg/float64.hpp" +#include "tf2_ros/transform_broadcaster.h" +// #include "vehicles/vtec_sdc1.hpp" +#include "dynamic_models/ground_vehicles/car_like/vehicles/vtec_sdc1.hpp" + +using namespace std::chrono_literals; + +class SdvDynNode : public rclcpp::Node { + public: + SdvDynNode() : Node("dyn_sdv") { + using namespace std::placeholders; + + this->declare_parameter("sample_time", rclcpp::PARAMETER_DOUBLE); // Super important to get parameters from launch files!! + this->declare_parameter("D_MAX", rclcpp::PARAMETER_INTEGER); + sample_time_ = this->get_parameter("sample_time").as_double(); + D_MAX_ = this->get_parameter("D_MAX").as_int(); + // this->get_parameter_or("sample_time", sample_time_, 0.01); + // this->get_parameter_or("D_MAX", D_MAX_, static_cast(255)); + + model = VTecSDC1DynamicModel{sample_time_, D_MAX_}; + model.setInitPose(Eigen::Vector3f{0,0,0}); + + odom_pub_ = + this->create_publisher("output/odom", 10); + + pose_path_pub_ = this->create_publisher( + "/sdv/pose_path", 10); + + throttle_sub_ = this->create_subscription( + "/sdv/velocity/throttle", 10, + [this](const std_msgs::msg::UInt8 &msg) { + car_model_.setThrottle(msg.data); + }); + + tf_broadcaster_ = std::make_unique(*this); + pose_stamped_tmp_.header.frame_id = "world"; + pose_path.header.frame_id = "world"; + pose_path.header.stamp = SdvDynNode::now(); + + // sample_time_ = 0.01; + updateTimer = this->create_wall_timer( + 1000ms*sample_time_, std::bind(&SdvDynNode::update, this)); + } + + protected: + void update() { + car_model_.calculateStates(); + car_model_.calculateModelParams(); + + /** + * Output stage + */ + double x = car_model_.eta_pose_(0); // position in x + double y = car_model_.eta_pose_(1); // position in y + double etheta = car_model_.eta_pose_(2); + + tf2::Quaternion q; + q.setRPY(0, 0, etheta); + + geometry_msgs::msg::Pose2D pose; + nav_msgs::msg::Odometry odom; + + etheta = std::fmod(etheta + M_PI, 2*M_PI) - M_PI; // [-pi, pi] + + pose.x = x; + pose.y = y; + pose.theta = etheta; + + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0; + odom.pose.pose.orientation.x = q[0]; + odom.pose.pose.orientation.y = q[1]; + odom.pose.pose.orientation.z = q[2]; + odom.pose.pose.orientation.w = q[3]; + + double u, v, r; + + geometry_msgs::msg::Vector3 velMsg; + + u = car_model_.velocities_(0); // surge velocity + v = car_model_.velocities_(1); // sway velocity + r = car_model_.velocities_(2); // yaw rate + velMsg.x = u; + velMsg.y = v; + velMsg.z = r; + odom.twist.twist.linear.x = u; + odom.twist.twist.linear.y = v; + odom.twist.twist.linear.z = 0; + + odom.twist.twist.angular.x = 0; + odom.twist.twist.angular.y = 0; + odom.twist.twist.angular.z = r; + + pose_stamped_tmp_.pose.position.x = pose.x; + pose_stamped_tmp_.pose.position.y = pose.y; + pose_path.poses.push_back(pose_stamped_tmp_); + + odom_pub_->publish(odom); + pose_path_pub_->publish(pose_path); + + tf_broadcast(pose); + } + + private: + rclcpp::Publisher::SharedPtr localVelPub; + rclcpp::Publisher::SharedPtr pose_path_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::TimerBase::SharedPtr updateTimer; + + geometry_msgs::msg::PoseStamped pose_stamped_tmp_; + nav_msgs::msg::Path pose_path; + + rclcpp::Subscription::SharedPtr throttle_sub_, + rightThrusterSub; + double Tport{0}, Tstbd{0}; + float sample_time_; + int D_MAX_; + + VTecSDC1DynamicModel model; + + std::unique_ptr tf_broadcaster_; + + void tf_broadcast(const geometry_msgs::msg::Pose2D &msg) { + geometry_msgs::msg::TransformStamped t; + + // Read message content and assign it to + // corresponding tf variables + t.header.stamp = this->get_clock()->now(); + t.header.frame_id = "world"; + t.child_frame_id = "usv"; + + // Turtle only exists in 2D, thus we get x and y translation + // coordinates from the message and set the z coordinate to 0 + t.transform.translation.x = msg.x; + t.transform.translation.y = msg.y; + t.transform.translation.z = 0.0; + + // For the same reason, turtle can only rotate around one axis + // and this why we set rotation in x and y to 0 and obtain + // rotation in z axis from the message + tf2::Quaternion q; + q.setRPY(0, 0, msg.theta); + t.transform.rotation.x = q.x(); + t.transform.rotation.y = q.y(); + t.transform.rotation.z = q.z(); + t.transform.rotation.w = q.w(); + + // Send the transformation + tf_broadcaster_->sendTransform(t); + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file From 2a006f5068334ed423938a1db51018887077ed86 Mon Sep 17 00:00:00 2001 From: MaxPacheco02 Date: Sat, 23 Nov 2024 11:02:58 -0600 Subject: [PATCH 2/4] checkpoint, not finished, needs a more drastic rewrite --- workspace/src/sdv_control/CMakeLists.txt | 14 +- .../src/sdv_control/config/car_params.yaml | 22 +- .../launch/dynamic_model_sim_launch.py | 49 +++++ .../launch/longitudinal_control_sim_launch.py | 57 +++++ .../src/sdv_control/launch/model_launch.py | 70 ------ .../src/sdv_control/libs/vanttec_controllers | 2 +- workspace/src/sdv_control/package.xml | 7 + .../first_order/sdv_vel_pid_node.cpp | 128 +++++++++++ .../first_order/sdv_vel_pid_node_v1.cpp | 128 +++++++++++ .../regulators/first_order/vel_pid_node.cpp | 2 +- .../first_order/velocity_regulator_node.cpp | 2 +- .../src/model/car_tf2_broadcast_node.cpp | 86 -------- .../src/model/sdc1_simulation_node.cpp | 122 ---------- ...amic_node.cpp => sdv_dynamic_sim_node.cpp} | 64 +++--- workspace/src/sdv_description/CMakeLists.txt | 9 +- .../{rviz.launch.py => old_rviz.launch.py} | 0 .../src/sdv_description/launch/rviz_launch.py | 40 ++++ workspace/src/sdv_description/rviz/urdf.rviz | 208 ++++++++++++++++++ workspace/src/sdv_description/urdf/sdv.urdf | 8 +- .../src/sdv_description/urdf/sdv_simple.urdf | 21 ++ workspace/src/sdv_msgs/CMakeLists.txt | 1 + .../src/sdv_msgs/msg/NonlinearFunctions.msg | 2 + 22 files changed, 714 insertions(+), 328 deletions(-) create mode 100755 workspace/src/sdv_control/launch/dynamic_model_sim_launch.py create mode 100644 workspace/src/sdv_control/launch/longitudinal_control_sim_launch.py delete mode 100755 workspace/src/sdv_control/launch/model_launch.py create mode 100644 workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node.cpp create mode 100644 workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node_v1.cpp delete mode 100644 workspace/src/sdv_control/src/model/car_tf2_broadcast_node.cpp delete mode 100644 workspace/src/sdv_control/src/model/sdc1_simulation_node.cpp rename workspace/src/sdv_control/src/model/{sdv_dynamic_node.cpp => sdv_dynamic_sim_node.cpp} (71%) rename workspace/src/sdv_description/launch/{rviz.launch.py => old_rviz.launch.py} (100%) create mode 100644 workspace/src/sdv_description/launch/rviz_launch.py create mode 100644 workspace/src/sdv_description/rviz/urdf.rviz create mode 100644 workspace/src/sdv_description/urdf/sdv_simple.urdf create mode 100644 workspace/src/sdv_msgs/msg/NonlinearFunctions.msg diff --git a/workspace/src/sdv_control/CMakeLists.txt b/workspace/src/sdv_control/CMakeLists.txt index 1358a879..1a02b73a 100644 --- a/workspace/src/sdv_control/CMakeLists.txt +++ b/workspace/src/sdv_control/CMakeLists.txt @@ -40,7 +40,7 @@ set(DEPENDENCIES add_subdirectory(libs/vanttec_controllers) ament_python_install_package(scripts) -add_executable(vel_pid_node src/control/regulators/first_order/vel_pid_node.cpp) +add_executable(vel_pid_node src/control/regulators/first_order/sdv_vel_pid_node.cpp) target_include_directories(vel_pid_node PUBLIC $ $ libs/vanttec_controllers) @@ -64,13 +64,13 @@ target_compile_features(sdv_kinematic_sim PUBLIC c_std_99 cxx_std_17) ament_target_dependencies(sdv_kinematic_sim ${DEPENDENCIES}) target_link_libraries(sdv_kinematic_sim vanttec_controllers) -add_executable(sdv_dynamic_node src/model/sdv_dynamic_node.cpp) -target_include_directories(sdv_dynamic_node PUBLIC +add_executable(sdv_dynamic_sim_node src/model/sdv_dynamic_sim_node.cpp) +target_include_directories(sdv_dynamic_sim_node PUBLIC $ $ libs/vanttec_controllers) -target_compile_features(sdv_dynamic_node PUBLIC c_std_99 cxx_std_17) -ament_target_dependencies(sdv_dynamic_node ${DEPENDENCIES}) -target_link_libraries(sdv_dynamic_node vanttec_controllers) +target_compile_features(sdv_dynamic_sim_node PUBLIC c_std_99 cxx_std_17) +ament_target_dependencies(sdv_dynamic_sim_node ${DEPENDENCIES}) +target_link_libraries(sdv_dynamic_sim_node vanttec_controllers) add_executable(velocity_regulator_node src/control/regulators/first_order/velocity_regulator_node.cpp) target_include_directories(velocity_regulator_node PUBLIC @@ -85,7 +85,7 @@ install(TARGETS stanley_controller_node sdv_kinematic_sim velocity_regulator_node - sdv_dynamic_node + sdv_dynamic_sim_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY diff --git a/workspace/src/sdv_control/config/car_params.yaml b/workspace/src/sdv_control/config/car_params.yaml index 9fcc17e1..62134608 100644 --- a/workspace/src/sdv_control/config/car_params.yaml +++ b/workspace/src/sdv_control/config/car_params.yaml @@ -9,13 +9,23 @@ # PID Controller for the VTec SDV vel_pid_node: ros__parameters: - Kp: 1.0 - Ki: 0.01 - Kd: 0.2 - U_MAX: 1.0 - U_MIN: -1.0 + Kp: 10. + Ki: 0.0 + Kd: 10. + U_MAX: 180.0 + U_MIN: -180.0 enable_ramp: true - ramp_rate: 0.3 + ramp_rate: 0.1 +# Simple version +# vel_pid_node: +# ros__parameters: +# Kp: 1.0 +# Ki: 0.01 +# Kd: 0.2 +# U_MAX: 1.0 +# U_MIN: -1.0 +# enable_ramp: true +# ramp_rate: 0.3 # First Order Feedback Linearization 1-DOF PID Controller for the VTec SDC1 sdc1_vel_pid_node: diff --git a/workspace/src/sdv_control/launch/dynamic_model_sim_launch.py b/workspace/src/sdv_control/launch/dynamic_model_sim_launch.py new file mode 100755 index 00000000..bb67c218 --- /dev/null +++ b/workspace/src/sdv_control/launch/dynamic_model_sim_launch.py @@ -0,0 +1,49 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +from launch.actions import IncludeLaunchDescription, LogInfo, DeclareLaunchArgument +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.substitutions import FindPackageShare + +from launch.substitutions import FindExecutable +from launch.actions import ExecuteProcess +import os + +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from ament_index_python.packages import get_package_share_path + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription, LogInfo +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import PathJoinSubstitution +from launch.conditions import IfCondition, UnlessCondition +from launch.substitutions import Command, LaunchConfiguration + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare +def generate_launch_description(): + dynamic_model = Node( + package="sdv_control", + executable="sdv_dynamic_sim_node", + ) + + rviz = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('sdv_description'), + 'launch', + 'rviz_launch.py' + ]) + ]), + ) + + return LaunchDescription([ + rviz, + dynamic_model, + ]) diff --git a/workspace/src/sdv_control/launch/longitudinal_control_sim_launch.py b/workspace/src/sdv_control/launch/longitudinal_control_sim_launch.py new file mode 100644 index 00000000..a280b228 --- /dev/null +++ b/workspace/src/sdv_control/launch/longitudinal_control_sim_launch.py @@ -0,0 +1,57 @@ +import os + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import IncludeLaunchDescription, LogInfo, DeclareLaunchArgument +from launch.substitutions import PathJoinSubstitution, LaunchConfiguration +from launch.launch_description_sources import PythonLaunchDescriptionSource + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + +from launch.conditions import IfCondition, UnlessCondition + +def generate_launch_description(): + + car_params = os.path.join( + get_package_share_directory('sdv_control'), + 'config', + 'car_params.yaml' + ) + + pid_node = Node( + package='sdv_control', + executable='vel_pid_node', + parameters=[car_params], + output='screen', + emulate_tty=True, + arguments=[('__log_level:=debug')], + ) + + foxglove_bridge = Node( + name="foxglove_bridge", + package="foxglove_bridge", + executable="foxglove_bridge") + + can_node = Node( + package='sdv_can', + executable='sdv_can_node', + ) + + dynamic_model_sim_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource([ + PathJoinSubstitution([ + FindPackageShare('sdv_control'), + 'launch', + 'dynamic_model_sim_launch.py' + ]) + ]) + ) + + return LaunchDescription([ + pid_node, + foxglove_bridge, + # can_node, + dynamic_model_sim_launch, + ]) \ No newline at end of file diff --git a/workspace/src/sdv_control/launch/model_launch.py b/workspace/src/sdv_control/launch/model_launch.py deleted file mode 100755 index f14c9443..00000000 --- a/workspace/src/sdv_control/launch/model_launch.py +++ /dev/null @@ -1,70 +0,0 @@ -import os -from launch import LaunchDescription -from launch_ros.actions import Node -from launch.actions import DeclareLaunchArgument -from launch.substitutions import LaunchConfiguration -from ament_index_python.packages import get_package_share_directory - -def generate_launch_description(): - frequency_arg = DeclareLaunchArgument( - name='frequency', - default_value='100', - description='Frequency for nodes' - ) - - rviz_config = os.path.join( - get_package_share_directory('sdv_control'), - 'launch/rviz_cfg', - 'sdv.rviz' - ) - - car_model_node = Node( - package='sdv_control', - executable='sdc1_simulation_node', - output='screen', - name='sdc1_simulation_node', - parameters=[{'frequency': LaunchConfiguration('frequency')}] - ) - - tf2_node = Node( - package='sdv_control', - executable='car_tf2_broadcast_node', - namespace="", - name='car_tf2_broadcast_node', - parameters=[{'frequency': LaunchConfiguration('frequency')}] - ) - - rviz = Node( - package='rviz2', - executable='rviz2', - name='rviz2', - # arguments=['-d', rviz_config] - ) - - return LaunchDescription([ - frequency_arg, - car_model_node, - # tf2_node, - # rviz - ]) - - - # return LaunchDescription([ - # nodes_frequency, - - # launch_ros.actions.Node( - # package='sdv_manual_control', - # executable='sdv_manual_control', - # name=['sdv_manual_control', launch.substitutions.LaunchConfiguration('role_name')], - # output='screen', - # emulate_tty=True, - # parameters=[ - # { - # 'role_name': launch.substitutions.LaunchConfiguration('role_name') - # } - # ] - # ), - - # ]) - - \ No newline at end of file diff --git a/workspace/src/sdv_control/libs/vanttec_controllers b/workspace/src/sdv_control/libs/vanttec_controllers index 35134a08..67d577d8 160000 --- a/workspace/src/sdv_control/libs/vanttec_controllers +++ b/workspace/src/sdv_control/libs/vanttec_controllers @@ -1 +1 @@ -Subproject commit 35134a08f0fc92a3540672d54ecda58c6c2f20c5 +Subproject commit 67d577d8233fb67acfbb25a33120f71964af12f2 diff --git a/workspace/src/sdv_control/package.xml b/workspace/src/sdv_control/package.xml index e54da11d..a55bd54b 100644 --- a/workspace/src/sdv_control/package.xml +++ b/workspace/src/sdv_control/package.xml @@ -7,6 +7,7 @@ root TODO: License declaration + ament_cmake ament_cmake_ros ament_cmake_python @@ -23,6 +24,12 @@ rclcpp rclpy + launch + launch_ros + visualization_msgs + tf2_geometry_msgs + ros2launch + ament_lint_auto ament_lint_common diff --git a/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node.cpp b/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node.cpp new file mode 100644 index 00000000..4bbf0bf5 --- /dev/null +++ b/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node.cpp @@ -0,0 +1,128 @@ +#include +#include +#include + +#include "geometry_msgs/msg/pose2_d.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "std_msgs/msg/float64.hpp" +#include "std_msgs/msg/u_int16.hpp" + +#include +#include "rclcpp/rclcpp.hpp" + +#include "utils/utils.hpp" + +#include "geometry_msgs/msg/accel.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/multi_array_dimension.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/u_int8.hpp" +#include "std_msgs/msg/string.hpp" + +#include "sdv_msgs/msg/nonlinear_functions.hpp" +#include "vectornav_msgs/msg/common_group.hpp" +#include "vectornav_msgs/msg/ins_group.hpp" + +#include "controllers/feedback_linearization/model_based_controllers/SDCs/regulators/vtec_sdc1_pid.cpp" + +using namespace std::chrono_literals; + +class VelPidNode : public rclcpp::Node { + public: + VelPidNode() : Node("vel_pid_node") { + using namespace std::placeholders; + params = initialize_params(); + controller_ = VTEC_SDC1_1DOF_PID(params); + + velocity_setpoint_sub_ = this->create_subscription( + "/sdv/velocity/setpoint", 10, + [this](const std_msgs::msg::Float64 &msg) { vel_d_ = msg.data; }); + + velocity_sub_ = this->create_subscription( + "/vectornav/velocity_body", 10, + [this](const nav_msgs::msg::Odometry &msg) { + vel_ = msg.twist.twist.linear.x; + }); + + f_g_sub_ = this->create_subscription( + "/sdv/control/nonlinear_functions", 10, + [this](const sdv_msgs::msg::NonlinearFunctions &msg) { + f_x_ = msg.f_x; + g_x_ = msg.g_x; + }); + + throttle_pub_ = this->create_publisher( + "/sdv/velocity/throttle", 10); + + + updateTimer = + this->create_wall_timer(10ms, std::bind(&VelPidNode::update, this)); + } + + private: + PIDParameters params; + // PID controller{PID::defaultParams()}; + + rclcpp::Subscription::SharedPtr velocity_setpoint_sub_; + rclcpp::Subscription::SharedPtr velocity_sub_; + rclcpp::Subscription::SharedPtr f_g_sub_; + + rclcpp::Publisher::SharedPtr throttle_pub_; + + rclcpp::TimerBase::SharedPtr updateTimer; + + double vel_{0}, vel_d_{0}, u_{0}; + double f_x_{0}, g_x_{0}; + + std_msgs::msg::Float64 throttle_msg; + + DOFControllerType_E controller_type_{LINEAR_DOF}; + VTEC_SDC1_1DOF_PID controller_{params}; + + PIDParameters initialize_params() { + this->declare_parameter("Kp", 1.0); + this->declare_parameter("Ki", 0.01); + this->declare_parameter("Kd", 0.2); + this->declare_parameter("U_MAX", 1.0); + this->declare_parameter("U_MIN", -1.0); + this->declare_parameter("enable_ramp", true); + this->declare_parameter("ramp_rate", 1.0); + + PIDParameters p; + p.kP = this->get_parameter("Kp").as_double(); + p.kI = this->get_parameter("Ki").as_double(); + p.kD = this->get_parameter("Kd").as_double(); + p.kDt = 0.01; + p.kUMax = this->get_parameter("U_MAX").as_double(); + p.kUMin = this->get_parameter("U_MIN").as_double(); + p.enable_ramp_rate_limit = this->get_parameter("enable_ramp").as_bool(); + p.ramp_rate = this->get_parameter("ramp_rate").as_double(); + return p; + } + + void update() { + controller_.updateNonLinearFunctions(f_x_, g_x_); + u_ = controller_.calculateControlSignals(vel_, vel_d_); + // u_ = controller_.get_control_signal(); + controller_.updateDBSignals(vel_, vel_d_, u_); + double d_ = controller_.get_D_(); + std::cout << "vel: " << vel_ << std::endl; + + // if(u_ < -0.3 || u_ > 0.2) { + if(d_ < -0. || d_ > 0.) { + throttle_msg.data = d_; + } else { + throttle_msg.data = 0.0; + } + throttle_pub_->publish(throttle_msg); + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node_v1.cpp b/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node_v1.cpp new file mode 100644 index 00000000..4bbf0bf5 --- /dev/null +++ b/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node_v1.cpp @@ -0,0 +1,128 @@ +#include +#include +#include + +#include "geometry_msgs/msg/pose2_d.hpp" +#include "geometry_msgs/msg/vector3.hpp" +#include "std_msgs/msg/float64.hpp" +#include "std_msgs/msg/u_int16.hpp" + +#include +#include "rclcpp/rclcpp.hpp" + +#include "utils/utils.hpp" + +#include "geometry_msgs/msg/accel.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_with_covariance_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/multi_array_dimension.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/u_int8.hpp" +#include "std_msgs/msg/string.hpp" + +#include "sdv_msgs/msg/nonlinear_functions.hpp" +#include "vectornav_msgs/msg/common_group.hpp" +#include "vectornav_msgs/msg/ins_group.hpp" + +#include "controllers/feedback_linearization/model_based_controllers/SDCs/regulators/vtec_sdc1_pid.cpp" + +using namespace std::chrono_literals; + +class VelPidNode : public rclcpp::Node { + public: + VelPidNode() : Node("vel_pid_node") { + using namespace std::placeholders; + params = initialize_params(); + controller_ = VTEC_SDC1_1DOF_PID(params); + + velocity_setpoint_sub_ = this->create_subscription( + "/sdv/velocity/setpoint", 10, + [this](const std_msgs::msg::Float64 &msg) { vel_d_ = msg.data; }); + + velocity_sub_ = this->create_subscription( + "/vectornav/velocity_body", 10, + [this](const nav_msgs::msg::Odometry &msg) { + vel_ = msg.twist.twist.linear.x; + }); + + f_g_sub_ = this->create_subscription( + "/sdv/control/nonlinear_functions", 10, + [this](const sdv_msgs::msg::NonlinearFunctions &msg) { + f_x_ = msg.f_x; + g_x_ = msg.g_x; + }); + + throttle_pub_ = this->create_publisher( + "/sdv/velocity/throttle", 10); + + + updateTimer = + this->create_wall_timer(10ms, std::bind(&VelPidNode::update, this)); + } + + private: + PIDParameters params; + // PID controller{PID::defaultParams()}; + + rclcpp::Subscription::SharedPtr velocity_setpoint_sub_; + rclcpp::Subscription::SharedPtr velocity_sub_; + rclcpp::Subscription::SharedPtr f_g_sub_; + + rclcpp::Publisher::SharedPtr throttle_pub_; + + rclcpp::TimerBase::SharedPtr updateTimer; + + double vel_{0}, vel_d_{0}, u_{0}; + double f_x_{0}, g_x_{0}; + + std_msgs::msg::Float64 throttle_msg; + + DOFControllerType_E controller_type_{LINEAR_DOF}; + VTEC_SDC1_1DOF_PID controller_{params}; + + PIDParameters initialize_params() { + this->declare_parameter("Kp", 1.0); + this->declare_parameter("Ki", 0.01); + this->declare_parameter("Kd", 0.2); + this->declare_parameter("U_MAX", 1.0); + this->declare_parameter("U_MIN", -1.0); + this->declare_parameter("enable_ramp", true); + this->declare_parameter("ramp_rate", 1.0); + + PIDParameters p; + p.kP = this->get_parameter("Kp").as_double(); + p.kI = this->get_parameter("Ki").as_double(); + p.kD = this->get_parameter("Kd").as_double(); + p.kDt = 0.01; + p.kUMax = this->get_parameter("U_MAX").as_double(); + p.kUMin = this->get_parameter("U_MIN").as_double(); + p.enable_ramp_rate_limit = this->get_parameter("enable_ramp").as_bool(); + p.ramp_rate = this->get_parameter("ramp_rate").as_double(); + return p; + } + + void update() { + controller_.updateNonLinearFunctions(f_x_, g_x_); + u_ = controller_.calculateControlSignals(vel_, vel_d_); + // u_ = controller_.get_control_signal(); + controller_.updateDBSignals(vel_, vel_d_, u_); + double d_ = controller_.get_D_(); + std::cout << "vel: " << vel_ << std::endl; + + // if(u_ < -0.3 || u_ > 0.2) { + if(d_ < -0. || d_ > 0.) { + throttle_msg.data = d_; + } else { + throttle_msg.data = 0.0; + } + throttle_pub_->publish(throttle_msg); + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/workspace/src/sdv_control/src/control/regulators/first_order/vel_pid_node.cpp b/workspace/src/sdv_control/src/control/regulators/first_order/vel_pid_node.cpp index 8df676be..d787eadb 100644 --- a/workspace/src/sdv_control/src/control/regulators/first_order/vel_pid_node.cpp +++ b/workspace/src/sdv_control/src/control/regulators/first_order/vel_pid_node.cpp @@ -12,7 +12,7 @@ #include "controllers/control_laws/PID/first_order/pid.hpp" -#include "utils/utils.hpp" +// #include "utils/utils.hpp" #include "geometry_msgs/msg/accel.hpp" #include "geometry_msgs/msg/twist.hpp" diff --git a/workspace/src/sdv_control/src/control/regulators/first_order/velocity_regulator_node.cpp b/workspace/src/sdv_control/src/control/regulators/first_order/velocity_regulator_node.cpp index 10fde1ab..71ef1bc9 100644 --- a/workspace/src/sdv_control/src/control/regulators/first_order/velocity_regulator_node.cpp +++ b/workspace/src/sdv_control/src/control/regulators/first_order/velocity_regulator_node.cpp @@ -6,7 +6,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "utils/utils.hpp" +// #include "utils/utils.hpp" using namespace std::chrono_literals; diff --git a/workspace/src/sdv_control/src/model/car_tf2_broadcast_node.cpp b/workspace/src/sdv_control/src/model/car_tf2_broadcast_node.cpp deleted file mode 100644 index 4a334f12..00000000 --- a/workspace/src/sdv_control/src/model/car_tf2_broadcast_node.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/** ---------------------------------------------------------------------------- - * @file: car_tf2_broadcast_node.cpp - * @date: August 12, 2023 - * @author: Sebas Mtz - * @author: Edison Altamirano - * - * @brief: tf2 broadcast node for RViz - * ----------------------------------------------------------------------------- - **/ - -#include -#include -#include "rclcpp/rclcpp.hpp" - -#include "simulation/car_tfs/sdc1_broadcaster.hpp" - -#include "nav_msgs/msg/path.hpp" - -#include "sdv_msgs/msg/eta_pose.hpp" - -using namespace std::chrono_literals; - -class CarTf2Broadcast : public rclcpp::Node -{ - private: - int frequency_; - sdv_msgs::msg::EtaPose car_pose_; - - rclcpp::TimerBase::SharedPtr timer_; - std::unique_ptr tf_broadcaster_; - - rclcpp::Publisher::SharedPtr car_path_; - - rclcpp::Subscription::SharedPtr car_eta_pose_; - - std::string parent_frame_; - std::string child_frame_; - - void timer_callback() - { - tf_broadcaster_->broadcastTransform(car_pose_); - car_path_->publish(tf_broadcaster_->path_); - } - - void set_sim_pose(const sdv_msgs::msg::EtaPose& msg) - { - car_pose_.x = msg.x; - car_pose_.y = msg.y; - car_pose_.psi = msg.psi; - } - - public: - CarTf2Broadcast() : Node("car_t2_broadcast_node") - { - this->declare_parameter("frequency", rclcpp::PARAMETER_INTEGER); // Super important to get parameters from launch files!! - - this->get_parameter_or("frequency", frequency_, 100); - - car_path_ = this->create_publisher("/sdc_simulation/car_tf_broadcast/car_path", 10); - - car_eta_pose_ = this->create_subscription("/sdc_simulation/dynamic_model/eta_pose", - 1, std::bind(&CarTf2Broadcast::set_sim_pose, this, std::placeholders::_1)); - - timer_ = this->create_wall_timer( - std::chrono::milliseconds(1000 / frequency_), - std::bind(&CarTf2Broadcast::timer_callback, this)); - } - - ~CarTf2Broadcast(){tf_broadcaster_.reset();} - - void configure() - { - tf_broadcaster_ = std::make_unique(shared_from_this()); - } - -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto tf2_node = std::make_shared(); - tf2_node->configure(); - rclcpp::spin(tf2_node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/workspace/src/sdv_control/src/model/sdc1_simulation_node.cpp b/workspace/src/sdv_control/src/model/sdc1_simulation_node.cpp deleted file mode 100644 index 57aef503..00000000 --- a/workspace/src/sdv_control/src/model/sdc1_simulation_node.cpp +++ /dev/null @@ -1,122 +0,0 @@ -/** ---------------------------------------------------------------------------- - * @file: sdc1_simulation_node.cpp - * @date: August 12, 2023 - * @author: Sebas Mtz - * @email: sebas.martp@gmail.com - * @author: Edison Altamirano - * - * @brief: Self-Driving Car 1 simulation node. Based on Sebas' car dyn model. - * ----------------------------------------------------------------------------- - **/ - -#include -#include "rclcpp/rclcpp.hpp" - -#include "vehicles/vtec_sdc1.hpp" - -#include "geometry_msgs/msg/accel.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "std_msgs/msg/multi_array_dimension.hpp" -#include "std_msgs/msg/u_int8.hpp" -#include "diagnostic_msgs/msg/diagnostic_status.hpp" -#include "diagnostic_msgs/msg/key_value.hpp" - -#include "sdv_msgs/msg/eta_pose.hpp" - -class CarSimulationNode : public rclcpp::Node -{ - private: - float sample_time_; - uint8_t D_MAX_{255}; - - std::unique_ptr car_model_; - rclcpp::TimerBase::SharedPtr timer_; - diagnostic_msgs::msg::DiagnosticStatus throttle_diag_; - - rclcpp::Publisher::SharedPtr car_accel_; - rclcpp::Publisher::SharedPtr car_vel_; - rclcpp::Publisher::SharedPtr car_eta_pose_; - // rclcpp::Publisher::SharedPtr car_dynamics_; - rclcpp::Publisher::SharedPtr diagnostics_publisher_; - - rclcpp::Subscription::SharedPtr in_subscriber_; - - std::vector init_pose_ = {0,0,0}; - - void timer_callback() - { - /* calculate Model States */ - car_model_->calculateStates(); - - car_model_->calculateModelParams(); - - // car_model_->updateDBSignals(); - - /* Publish Odometry */ - car_accel_->publish(car_model_->accelerations_); - car_vel_->publish(car_model_->velocities_); - car_eta_pose_->publish(car_model_->eta_pose_); - - /* Publish diagnostics */ - diagnostic_msgs::msg::KeyValue value; - // TODO. Check for errors in throttle computation (maybe there are no real values) to updated diagnostics - throttle_diag_.level = 0; - value.key = "throttle_signal"; - // value.value = car_model_->u_(0); - value.value = car_model_->D_; - // RCLCPP_INFO(this->get_logger(), "U: %f", car_model_->u_(0)); - - throttle_diag_.values.push_back(value); - diagnostics_publisher_->publish(throttle_diag_); - } - - void force_callback(const std_msgs::msg::UInt8& msg) const - { - car_model_->setThrottle(msg.data); - } - - public: - CarSimulationNode() : Node("sdc1_simulation_node") - { - int frequency; - this->declare_parameter("frequency", rclcpp::PARAMETER_INTEGER); // Super important to get parameters from launch files!! - this->declare_parameter("D_MAX", rclcpp::PARAMETER_INTEGER); - this->get_parameter_or("frequency", frequency, 100); - this->get_parameter_or("D_MAX", D_MAX_, static_cast(255)); - - sample_time_ = 1.0 / static_cast(frequency); - - car_accel_ = this->create_publisher("/sdc_simulation/dynamic_model/accel", 10); - car_vel_ = this->create_publisher("/sdc_simulation/dynamic_model/vel", 10); - car_eta_pose_ = this->create_publisher("/sdc_simulation/dynamic_model/eta_pose", 10); - diagnostics_publisher_ = this->create_publisher("/diagnostics",10); - - in_subscriber_ = this->create_subscription("/sdc_simulation/dynamic_model/set_throttle", - 1, std::bind(&CarSimulationNode::force_callback, this, std::placeholders::_1)); - - throttle_diag_.name = "Throttle command (D)"; - throttle_diag_.message = "Expected value must be integer in the range of [0, 255]"; - throttle_diag_.hardware_id = "Throttle"; - - timer_ = this->create_wall_timer( std::chrono::milliseconds(1000 / frequency), - std::bind(&CarSimulationNode::timer_callback, this)); - } - - ~CarSimulationNode(){car_model_.reset();} - - void configure() - { - car_model_ = std::make_unique(sample_time_, D_MAX_); - car_model_->setInitPose(init_pose_); - } -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - auto model_node = std::make_shared(); - model_node->configure(); - rclcpp::spin(model_node->get_node_base_interface()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/workspace/src/sdv_control/src/model/sdv_dynamic_node.cpp b/workspace/src/sdv_control/src/model/sdv_dynamic_sim_node.cpp similarity index 71% rename from workspace/src/sdv_control/src/model/sdv_dynamic_node.cpp rename to workspace/src/sdv_control/src/model/sdv_dynamic_sim_node.cpp index 19590a80..0d71e068 100644 --- a/workspace/src/sdv_control/src/model/sdv_dynamic_node.cpp +++ b/workspace/src/sdv_control/src/model/sdv_dynamic_sim_node.cpp @@ -16,59 +16,63 @@ #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/float64.hpp" #include "tf2_ros/transform_broadcaster.h" -// #include "vehicles/vtec_sdc1.hpp" -#include "dynamic_models/ground_vehicles/car_like/vehicles/vtec_sdc1.hpp" +#include "sdv_msgs/msg/nonlinear_functions.hpp" + +#include "dynamic_models/ground_vehicles/car_like/vehicles/vtec_sdc1.cpp" using namespace std::chrono_literals; -class SdvDynNode : public rclcpp::Node { +class SDVDynamicSimNode : public rclcpp::Node { public: - SdvDynNode() : Node("dyn_sdv") { + SDVDynamicSimNode() : Node("sdv_dynamic_sim_node") { using namespace std::placeholders; - this->declare_parameter("sample_time", rclcpp::PARAMETER_DOUBLE); // Super important to get parameters from launch files!! - this->declare_parameter("D_MAX", rclcpp::PARAMETER_INTEGER); + this->declare_parameter("sample_time", 0.01); + this->declare_parameter("D_MAX", 255); sample_time_ = this->get_parameter("sample_time").as_double(); D_MAX_ = this->get_parameter("D_MAX").as_int(); - // this->get_parameter_or("sample_time", sample_time_, 0.01); - // this->get_parameter_or("D_MAX", D_MAX_, static_cast(255)); model = VTecSDC1DynamicModel{sample_time_, D_MAX_}; model.setInitPose(Eigen::Vector3f{0,0,0}); odom_pub_ = - this->create_publisher("output/odom", 10); + this->create_publisher("/vectornav/velocity_body", 10); pose_path_pub_ = this->create_publisher( "/sdv/pose_path", 10); + + f_g_pub_ = this->create_publisher( + "/sdv/control/nonlinear_functions", 10); - throttle_sub_ = this->create_subscription( + throttle_sub_ = this->create_subscription( "/sdv/velocity/throttle", 10, - [this](const std_msgs::msg::UInt8 &msg) { - car_model_.setThrottle(msg.data); + [this](const std_msgs::msg::Float64 &msg) { + model.setThrottle(msg.data); }); tf_broadcaster_ = std::make_unique(*this); pose_stamped_tmp_.header.frame_id = "world"; pose_path.header.frame_id = "world"; - pose_path.header.stamp = SdvDynNode::now(); + pose_path.header.stamp = SDVDynamicSimNode::now(); // sample_time_ = 0.01; updateTimer = this->create_wall_timer( - 1000ms*sample_time_, std::bind(&SdvDynNode::update, this)); + // 1000ms*sample_time_, std::bind(&SDVDynamicSimNode::update, this)); + 10ms, std::bind(&SDVDynamicSimNode::update, this)); } protected: void update() { - car_model_.calculateStates(); - car_model_.calculateModelParams(); + model.calculateModelParams(); + model.calculateStates(); /** * Output stage */ - double x = car_model_.eta_pose_(0); // position in x - double y = car_model_.eta_pose_(1); // position in y - double etheta = car_model_.eta_pose_(2); + double x{0}, y{0}, etheta{0}; + x = model.eta_pose_(0); // position in x + y = model.eta_pose_(1); // position in y + etheta = model.eta_pose_(2); tf2::Quaternion q; q.setRPY(0, 0, etheta); @@ -90,13 +94,13 @@ class SdvDynNode : public rclcpp::Node { odom.pose.pose.orientation.z = q[2]; odom.pose.pose.orientation.w = q[3]; - double u, v, r; + double u{0}, v{0}, r{0}; geometry_msgs::msg::Vector3 velMsg; - u = car_model_.velocities_(0); // surge velocity - v = car_model_.velocities_(1); // sway velocity - r = car_model_.velocities_(2); // yaw rate + u = model.velocities_(0); // surge velocity + v = model.velocities_(1); // sway velocity + r = model.velocities_(2); // yaw rate velMsg.x = u; velMsg.y = v; velMsg.z = r; @@ -115,6 +119,10 @@ class SdvDynNode : public rclcpp::Node { odom_pub_->publish(odom); pose_path_pub_->publish(pose_path); + f_g_msg.f_x = model.get_f_(); + f_g_msg.g_x = model.get_g_(); + f_g_pub_->publish(f_g_msg); + tf_broadcast(pose); } @@ -122,10 +130,12 @@ class SdvDynNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr localVelPub; rclcpp::Publisher::SharedPtr pose_path_pub_; rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr f_g_pub_; rclcpp::TimerBase::SharedPtr updateTimer; geometry_msgs::msg::PoseStamped pose_stamped_tmp_; - nav_msgs::msg::Path pose_path; + nav_msgs::msg::Path pose_path; + sdv_msgs::msg::NonlinearFunctions f_g_msg; rclcpp::Subscription::SharedPtr throttle_sub_, rightThrusterSub; @@ -133,7 +143,7 @@ class SdvDynNode : public rclcpp::Node { float sample_time_; int D_MAX_; - VTecSDC1DynamicModel model; + VTecSDC1DynamicModel model{0.01,255}; std::unique_ptr tf_broadcaster_; @@ -144,7 +154,7 @@ class SdvDynNode : public rclcpp::Node { // corresponding tf variables t.header.stamp = this->get_clock()->now(); t.header.frame_id = "world"; - t.child_frame_id = "usv"; + t.child_frame_id = "sdv"; // Turtle only exists in 2D, thus we get x and y translation // coordinates from the message and set the z coordinate to 0 @@ -169,7 +179,7 @@ class SdvDynNode : public rclcpp::Node { int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file diff --git a/workspace/src/sdv_description/CMakeLists.txt b/workspace/src/sdv_description/CMakeLists.txt index 81537ced..0ce22fcc 100644 --- a/workspace/src/sdv_description/CMakeLists.txt +++ b/workspace/src/sdv_description/CMakeLists.txt @@ -19,9 +19,12 @@ find_package(ament_cmake REQUIRED) find_package(urdf REQUIRED) -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY urdf DESTINATION share/${PROJECT_NAME}) -install(DIRECTORY meshes DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY + launch + urdf + meshes + rviz + DESTINATION share/${PROJECT_NAME}) ################################################################################ # Macro for ament package diff --git a/workspace/src/sdv_description/launch/rviz.launch.py b/workspace/src/sdv_description/launch/old_rviz.launch.py similarity index 100% rename from workspace/src/sdv_description/launch/rviz.launch.py rename to workspace/src/sdv_description/launch/old_rviz.launch.py diff --git a/workspace/src/sdv_description/launch/rviz_launch.py b/workspace/src/sdv_description/launch/rviz_launch.py new file mode 100644 index 00000000..d51dac50 --- /dev/null +++ b/workspace/src/sdv_description/launch/rviz_launch.py @@ -0,0 +1,40 @@ +import os +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch.conditions import IfCondition, UnlessCondition + +def generate_launch_description(): + rviz_config = os.path.join(get_package_share_directory("sdv_description"),'rviz/','urdf.rviz') + + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config], + ) + + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + urdf_file_name = 'sdv_simple.urdf' + urdf = os.path.join( + get_package_share_directory("sdv_description"), "urdf/", urdf_file_name) + with open(urdf, 'r') as infp: + robot_desc = infp.read() + + return LaunchDescription([ + rviz, + DeclareLaunchArgument( + 'use_sim_time', + default_value='false', + description='Use simulation (Gazebo) clock if true'), + Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='robot_state_publisher', + output='screen', + parameters=[{'use_sim_time': use_sim_time, 'robot_description': robot_desc}], + arguments=[urdf]), + ]) \ No newline at end of file diff --git a/workspace/src/sdv_description/rviz/urdf.rviz b/workspace/src/sdv_description/rviz/urdf.rviz new file mode 100644 index 00000000..30ca7b62 --- /dev/null +++ b/workspace/src/sdv_description/rviz/urdf.rviz @@ -0,0 +1,208 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Grid1 + Splitter Ratio: 0.5 + Tree Height: 946 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 94; 92; 100 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 50 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + sdv: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 25; 255; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Lines + Line Width: 0.029999999329447746 + Name: Path + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sdv/pose_path + Value: true + Enabled: true + Global Options: + Background Color: 61; 56; 70 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 88.31387329101562 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 8.840577125549316 + Y: -2.2151339054107666 + Z: 5.527770519256592 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.5153985023498535 + Target Frame: + Value: Orbit (rviz_default_plugins) + Yaw: 2.995405673980713 + Saved: + - Angle: -1.5707999467849731 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Default_M2 + Near Clip Distance: 0.009999999776482582 + Scale: 39.444580078125 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 11.687281608581543 + Y: -6.997159481048584 +Window Geometry: + Displays: + collapsed: false + Height: 1136 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd0000000400000000000001560000043dfc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c00610079007301000000160000043d000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000043dfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a0056006900650077007301000000160000043d000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005a000000039fc0100000002fb0000000800540069006d00650000000000000005a0000002fb00fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000043d00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730000000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 0 + Y: 27 diff --git a/workspace/src/sdv_description/urdf/sdv.urdf b/workspace/src/sdv_description/urdf/sdv.urdf index 1874f014..d8b18b3d 100644 --- a/workspace/src/sdv_description/urdf/sdv.urdf +++ b/workspace/src/sdv_description/urdf/sdv.urdf @@ -70,16 +70,16 @@ - + - + - + @@ -103,7 +103,7 @@ - + Gazebo/White true diff --git a/workspace/src/sdv_description/urdf/sdv_simple.urdf b/workspace/src/sdv_description/urdf/sdv_simple.urdf new file mode 100644 index 00000000..da2af2cf --- /dev/null +++ b/workspace/src/sdv_description/urdf/sdv_simple.urdf @@ -0,0 +1,21 @@ + + + + + + + + + + + + + + + + + Gazebo/White + true + + + \ No newline at end of file diff --git a/workspace/src/sdv_msgs/CMakeLists.txt b/workspace/src/sdv_msgs/CMakeLists.txt index c6e1359a..b5cda161 100644 --- a/workspace/src/sdv_msgs/CMakeLists.txt +++ b/workspace/src/sdv_msgs/CMakeLists.txt @@ -13,6 +13,7 @@ find_package(std_msgs REQUIRED) rosidl_generate_interfaces(sdv_msgs "msg/PerceptionMode.msg" + "msg/NonlinearFunctions.msg" "msg/State.msg" "msg/PanelMsg.msg" "msg/XboxMsg.msg" diff --git a/workspace/src/sdv_msgs/msg/NonlinearFunctions.msg b/workspace/src/sdv_msgs/msg/NonlinearFunctions.msg new file mode 100644 index 00000000..00821f6d --- /dev/null +++ b/workspace/src/sdv_msgs/msg/NonlinearFunctions.msg @@ -0,0 +1,2 @@ +float64 f_x +float64 g_x \ No newline at end of file From bea422c917d6b79dcb5504db9ba2e9764ab04c34 Mon Sep 17 00:00:00 2001 From: MaxPacheco02 Date: Sat, 30 Nov 2024 12:10:43 -0600 Subject: [PATCH 3/4] temporal changes --- .../src/sdv_control/libs/vanttec_controllers | 2 +- .../regulators/first_order/controller.cpp | 21 ++++++ .../regulators/first_order/controller.h | 29 ++++++++ .../first_order/sdv_vel_pid_node.cpp | 66 +++++++------------ 4 files changed, 74 insertions(+), 44 deletions(-) create mode 100644 workspace/src/sdv_control/src/control/regulators/first_order/controller.cpp create mode 100644 workspace/src/sdv_control/src/control/regulators/first_order/controller.h diff --git a/workspace/src/sdv_control/libs/vanttec_controllers b/workspace/src/sdv_control/libs/vanttec_controllers index 67d577d8..f90851ab 160000 --- a/workspace/src/sdv_control/libs/vanttec_controllers +++ b/workspace/src/sdv_control/libs/vanttec_controllers @@ -1 +1 @@ -Subproject commit 67d577d8233fb67acfbb25a33120f71964af12f2 +Subproject commit f90851abb2e27087f91c1095bf2090aec01ae00a diff --git a/workspace/src/sdv_control/src/control/regulators/first_order/controller.cpp b/workspace/src/sdv_control/src/control/regulators/first_order/controller.cpp new file mode 100644 index 00000000..c355522b --- /dev/null +++ b/workspace/src/sdv_control/src/control/regulators/first_order/controller.cpp @@ -0,0 +1,21 @@ +#include "controller.h" +#include + +Controller::Controller(){ + a_x_ = 0; + f_x_ = 0; + g_x_ = 0; + u_aux_ = 0; + u_aux_dot_ = 0; + m_ = 0; + nu_ = 0; + D_ = 0; + + id = 0; + re_init(); +} + +SDVOuput Controller::update(const SDVUpdate ¶ms) +{ + return SDVOutput(); +} \ No newline at end of file diff --git a/workspace/src/sdv_control/src/control/regulators/first_order/controller.h b/workspace/src/sdv_control/src/control/regulators/first_order/controller.h new file mode 100644 index 00000000..f51c1b3d --- /dev/null +++ b/workspace/src/sdv_control/src/control/regulators/first_order/controller.h @@ -0,0 +1,29 @@ +#ifndef CONTROLLER_H +#define CONTROLLER_H + +struct SDVUpdate { + double f_x{0}; + double g_x{0}; + double nu{0}; + double u_aux{0}; +}; + +struct SDVOuput { + double huh{0}; +}; + +class Controller { + private: + double a_x_{0}; + double f_x_{0}, g_x_{0}; + double u_aux_{0}, u_aux_dot_{0}; + double m_{0}; + double nu_{0}; + double D_{0}; + + public: + Controller(); + SDVOuput update(const SDVUpdate ¶ms); +}; + +#endif // CONTROLLER_H diff --git a/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node.cpp b/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node.cpp index 4bbf0bf5..b30275a5 100644 --- a/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node.cpp +++ b/workspace/src/sdv_control/src/control/regulators/first_order/sdv_vel_pid_node.cpp @@ -10,7 +10,10 @@ #include #include "rclcpp/rclcpp.hpp" -#include "utils/utils.hpp" +#include "controllers/control_laws/PID/first_order/pid.hpp" +#include "controller.cpp" + +// #include "utils/utils.hpp" #include "geometry_msgs/msg/accel.hpp" #include "geometry_msgs/msg/twist.hpp" @@ -21,74 +24,58 @@ #include "std_msgs/msg/u_int8.hpp" #include "std_msgs/msg/string.hpp" -#include "sdv_msgs/msg/nonlinear_functions.hpp" +#include "sdv_msgs/msg/eta_pose.hpp" #include "vectornav_msgs/msg/common_group.hpp" #include "vectornav_msgs/msg/ins_group.hpp" -#include "controllers/feedback_linearization/model_based_controllers/SDCs/regulators/vtec_sdc1_pid.cpp" - using namespace std::chrono_literals; -class VelPidNode : public rclcpp::Node { +class SDV_VelPidNode : public rclcpp::Node { public: - VelPidNode() : Node("vel_pid_node") { + SDV_VelPidNode() : Node("sdv_vel_pid_node") { using namespace std::placeholders; params = initialize_params(); - controller_ = VTEC_SDC1_1DOF_PID(params); - + controller = PID(params); + velocity_setpoint_sub_ = this->create_subscription( "/sdv/velocity/setpoint", 10, - [this](const std_msgs::msg::Float64 &msg) { vel_d_ = msg.data; }); + [this](const std_msgs::msg::Float64 &msg) { vel_d = msg.data; }); velocity_sub_ = this->create_subscription( "/vectornav/velocity_body", 10, [this](const nav_msgs::msg::Odometry &msg) { - vel_ = msg.twist.twist.linear.x; - }); - - f_g_sub_ = this->create_subscription( - "/sdv/control/nonlinear_functions", 10, - [this](const sdv_msgs::msg::NonlinearFunctions &msg) { - f_x_ = msg.f_x; - g_x_ = msg.g_x; + vel = msg.twist.twist.linear.x; }); throttle_pub_ = this->create_publisher( "/sdv/velocity/throttle", 10); - updateTimer = - this->create_wall_timer(10ms, std::bind(&VelPidNode::update, this)); + this->create_wall_timer(10ms, std::bind(&SDV_VelPidNode::update, this)); } private: PIDParameters params; - // PID controller{PID::defaultParams()}; + PID controller{PID::defaultParams()}; rclcpp::Subscription::SharedPtr velocity_setpoint_sub_; rclcpp::Subscription::SharedPtr velocity_sub_; - rclcpp::Subscription::SharedPtr f_g_sub_; - rclcpp::Publisher::SharedPtr throttle_pub_; rclcpp::TimerBase::SharedPtr updateTimer; - double vel_{0}, vel_d_{0}, u_{0}; - double f_x_{0}, g_x_{0}; + double vel{0.0}, vel_d{0.0}, u_{0.0}; std_msgs::msg::Float64 throttle_msg; - DOFControllerType_E controller_type_{LINEAR_DOF}; - VTEC_SDC1_1DOF_PID controller_{params}; - PIDParameters initialize_params() { this->declare_parameter("Kp", 1.0); this->declare_parameter("Ki", 0.01); this->declare_parameter("Kd", 0.2); - this->declare_parameter("U_MAX", 1.0); - this->declare_parameter("U_MIN", -1.0); + this->declare_parameter("U_MAX", 180.0); + this->declare_parameter("U_MIN", -180.0); this->declare_parameter("enable_ramp", true); - this->declare_parameter("ramp_rate", 1.0); + this->declare_parameter("ramp_rate", 10.0); PIDParameters p; p.kP = this->get_parameter("Kp").as_double(); @@ -103,26 +90,19 @@ class VelPidNode : public rclcpp::Node { } void update() { - controller_.updateNonLinearFunctions(f_x_, g_x_); - u_ = controller_.calculateControlSignals(vel_, vel_d_); - // u_ = controller_.get_control_signal(); - controller_.updateDBSignals(vel_, vel_d_, u_); - double d_ = controller_.get_D_(); - std::cout << "vel: " << vel_ << std::endl; - - // if(u_ < -0.3 || u_ > 0.2) { - if(d_ < -0. || d_ > 0.) { - throttle_msg.data = d_; - } else { + u_ = controller.update(vel, vel_d); + // if(u_ < -0.3 || u_ > 0.2) + if(u_ < -0. || u_ > 0.) + throttle_msg.data = u_; + else throttle_msg.data = 0.0; - } throttle_pub_->publish(throttle_msg); } }; int main(int argc, char **argv) { rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); + rclcpp::spin(std::make_shared()); rclcpp::shutdown(); return 0; } \ No newline at end of file From bea23cc64fb919e72a269c8cc6a203a472795dfd Mon Sep 17 00:00:00 2001 From: vSebas Date: Wed, 29 Jan 2025 00:49:58 -0600 Subject: [PATCH 4/4] fblin pid now works again --- create_container_intel.bash | 2 +- workspace/src/sdv_control/CMakeLists.txt | 80 +++++++---- .../src/sdv_control/config/car_params.yaml | 14 +- .../src/sdv_control/launch/control_launch.py | 14 +- .../src/sdv_control/libs/vanttec_controllers | 2 +- .../first_order/sdc1_vel_pid_node.cpp | 136 +++++++++--------- .../src/model/car_tf2_broadcast_node.cpp | 86 +++++++++++ .../src/simulation/sdc1_broadcaster.cpp | 70 +++++++++ .../src/simulation/sdc1_broadcaster.hpp | 37 +++++ .../simulation/tf2_6dof_broadcaster_ros2.cpp | 31 ++++ .../simulation/tf2_6dof_broadcaster_ros2.hpp | 60 ++++++++ .../sdv_localization/CMakeLists.txt | 8 +- .../launch/sdv_localization.launch.py | 25 +++- 13 files changed, 437 insertions(+), 128 deletions(-) create mode 100644 workspace/src/sdv_control/src/model/car_tf2_broadcast_node.cpp create mode 100644 workspace/src/sdv_control/src/simulation/sdc1_broadcaster.cpp create mode 100644 workspace/src/sdv_control/src/simulation/sdc1_broadcaster.hpp create mode 100644 workspace/src/sdv_control/src/simulation/tf2_6dof_broadcaster_ros2.cpp create mode 100644 workspace/src/sdv_control/src/simulation/tf2_6dof_broadcaster_ros2.hpp diff --git a/create_container_intel.bash b/create_container_intel.bash index 84f3bc39..45ea213f 100755 --- a/create_container_intel.bash +++ b/create_container_intel.bash @@ -32,7 +32,7 @@ $DOCKER_COMMAND -it -d\ --privileged \ -v /dev/bus/usb/:/dev/bus/usb \ -v /dev:/dev \ - -v "$PWD/docker_ws:/home/ws/src" \ + -v "$PWD/workspace:/home/ws" \ -v "/media/saveasmtz/pacman/rosbags:/home/ws/src/tests" \ -v /var/run/docker.sock:/var/run/docker.sock \ -v /run/dbus:/run/dbus:ro \ diff --git a/workspace/src/sdv_control/CMakeLists.txt b/workspace/src/sdv_control/CMakeLists.txt index 1a02b73a..cc954eae 100644 --- a/workspace/src/sdv_control/CMakeLists.txt +++ b/workspace/src/sdv_control/CMakeLists.txt @@ -40,13 +40,25 @@ set(DEPENDENCIES add_subdirectory(libs/vanttec_controllers) ament_python_install_package(scripts) -add_executable(vel_pid_node src/control/regulators/first_order/sdv_vel_pid_node.cpp) -target_include_directories(vel_pid_node PUBLIC +add_library(tf2_lib SHARED + ${CMAKE_CURRENT_SOURCE_DIR}/src/simulation/sdc1_broadcaster.cpp + ${CMAKE_CURRENT_SOURCE_DIR}/src/simulation/tf2_6dof_broadcaster_ros2.cpp +) +ament_target_dependencies(tf2_lib ${DEPENDENCIES}) + +install(TARGETS tf2_lib + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +add_executable(sdc1_vel_pid_node src/control/regulators/first_order/sdc1_vel_pid_node.cpp) +target_include_directories(sdc1_vel_pid_node PUBLIC $ $ libs/vanttec_controllers) -target_compile_features(vel_pid_node PUBLIC c_std_99 cxx_std_17) -ament_target_dependencies(vel_pid_node ${DEPENDENCIES}) -target_link_libraries(vel_pid_node vanttec_controllers) +target_compile_features(sdc1_vel_pid_node PUBLIC c_std_99 cxx_std_17) +ament_target_dependencies(sdc1_vel_pid_node ${DEPENDENCIES}) +target_link_libraries(sdc1_vel_pid_node vanttec_controllers) add_executable(stanley_controller_node src/control/guidance_laws/stanley_controller_node.cpp) target_include_directories(stanley_controller_node PUBLIC @@ -56,36 +68,43 @@ target_compile_features(stanley_controller_node PUBLIC c_std_99 cxx_std_17) ament_target_dependencies(stanley_controller_node ${DEPENDENCIES}) target_link_libraries(stanley_controller_node vanttec_controllers) -add_executable(sdv_kinematic_sim src/model/sdv_kinematic_sim_node.cpp) -target_include_directories(sdv_kinematic_sim PUBLIC - $ - $ libs/vanttec_controllers) -target_compile_features(sdv_kinematic_sim PUBLIC c_std_99 cxx_std_17) -ament_target_dependencies(sdv_kinematic_sim ${DEPENDENCIES}) -target_link_libraries(sdv_kinematic_sim vanttec_controllers) +# add_executable(sdv_kinematic_sim src/model/sdv_kinematic_sim_node.cpp) +# target_include_directories(sdv_kinematic_sim PUBLIC +# $ +# $ libs/vanttec_controllers) +# target_compile_features(sdv_kinematic_sim PUBLIC c_std_99 cxx_std_17) +# ament_target_dependencies(sdv_kinematic_sim ${DEPENDENCIES}) +# target_link_libraries(sdv_kinematic_sim vanttec_controllers) -add_executable(sdv_dynamic_sim_node src/model/sdv_dynamic_sim_node.cpp) -target_include_directories(sdv_dynamic_sim_node PUBLIC - $ - $ libs/vanttec_controllers) -target_compile_features(sdv_dynamic_sim_node PUBLIC c_std_99 cxx_std_17) -ament_target_dependencies(sdv_dynamic_sim_node ${DEPENDENCIES}) -target_link_libraries(sdv_dynamic_sim_node vanttec_controllers) +# add_executable(velocity_regulator_node src/control/regulators/first_order/velocity_regulator_node.cpp) +# target_include_directories(velocity_regulator_node PUBLIC +# $ +# $ libs/vanttec_controllers) +# target_compile_features(velocity_regulator_node PUBLIC c_std_99 cxx_std_17) +# ament_target_dependencies(velocity_regulator_node ${DEPENDENCIES}) +# target_link_libraries(velocity_regulator_node vanttec_controllers) -add_executable(velocity_regulator_node src/control/regulators/first_order/velocity_regulator_node.cpp) -target_include_directories(velocity_regulator_node PUBLIC - $ - $ libs/vanttec_controllers) -target_compile_features(velocity_regulator_node PUBLIC c_std_99 cxx_std_17) -ament_target_dependencies(velocity_regulator_node ${DEPENDENCIES}) -target_link_libraries(velocity_regulator_node vanttec_controllers) +add_executable(car_tf2_broadcast_node src/model/car_tf2_broadcast_node.cpp) +# Include directories for project headers and other package headers +target_include_directories(car_tf2_broadcast_node + PUBLIC + # $ + $ + $ +) +target_compile_features(car_tf2_broadcast_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies(car_tf2_broadcast_node ${DEPENDENCIES}) +target_link_libraries(car_tf2_broadcast_node vanttec_controllers tf2_lib) +set_target_properties(car_tf2_broadcast_node PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/lib +) install(TARGETS - vel_pid_node + sdc1_vel_pid_node stanley_controller_node - sdv_kinematic_sim - velocity_regulator_node - sdv_dynamic_sim_node + # sdv_kinematic_sim + # velocity_regulator_node + car_tf2_broadcast_node DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY @@ -93,7 +112,6 @@ install(DIRECTORY launch rviz DESTINATION share/${PROJECT_NAME}) - # Install Python executables install(PROGRAMS scripts/path_publisher_node.py diff --git a/workspace/src/sdv_control/config/car_params.yaml b/workspace/src/sdv_control/config/car_params.yaml index 62134608..413b94be 100644 --- a/workspace/src/sdv_control/config/car_params.yaml +++ b/workspace/src/sdv_control/config/car_params.yaml @@ -2,7 +2,7 @@ ros__parameters: frequency: 100 init_pose: [0.0, 0.0, 0.0] # x, y, psi - init_pose: [-63.0, -99.0, 3.1415] # x, y, psi + # init_pose: [-63.0, -99.0, 3.1415] # x, y, psi parent_frame: "odom" child_frame: "base_link" @@ -16,16 +16,6 @@ vel_pid_node: U_MIN: -180.0 enable_ramp: true ramp_rate: 0.1 -# Simple version -# vel_pid_node: -# ros__parameters: -# Kp: 1.0 -# Ki: 0.01 -# Kd: 0.2 -# U_MAX: 1.0 -# U_MIN: -1.0 -# enable_ramp: true -# ramp_rate: 0.3 # First Order Feedback Linearization 1-DOF PID Controller for the VTec SDC1 sdc1_vel_pid_node: @@ -33,7 +23,7 @@ sdc1_vel_pid_node: Kp: 1.0 Ki: 0.01 Kd: 0.2 #0.01 - D_MAX: 180 + D_MAX: 90 # First Order Feedback Linearization 1-DOF ASMC Controller for the VTec SDC1 sdc1_vel_asmc_node: diff --git a/workspace/src/sdv_control/launch/control_launch.py b/workspace/src/sdv_control/launch/control_launch.py index f901ad6d..d138d614 100644 --- a/workspace/src/sdv_control/launch/control_launch.py +++ b/workspace/src/sdv_control/launch/control_launch.py @@ -16,7 +16,7 @@ def generate_launch_description(): is_sim = DeclareLaunchArgument( 'is_simulation', - default_value = 'false', + default_value = 'true', description = 'Defines if the application will run in simulation or in real life' ) @@ -165,16 +165,16 @@ def generate_launch_description(): msg="Running in real robot mode." ), - # rviz, + rviz, # aitsmc_node, # asmc_node, pid_node, - # tf2_node, - # sdv_description_launch, - # sdv_loc_launch, + tf2_node, + sdv_description_launch, + sdv_loc_launch, # sdv_can_launch, - waypoint_handler, - car_guidance_node, + # waypoint_handler, + # car_guidance_node, ]) \ No newline at end of file diff --git a/workspace/src/sdv_control/libs/vanttec_controllers b/workspace/src/sdv_control/libs/vanttec_controllers index f90851ab..56e2dc8f 160000 --- a/workspace/src/sdv_control/libs/vanttec_controllers +++ b/workspace/src/sdv_control/libs/vanttec_controllers @@ -1 +1 @@ -Subproject commit f90851abb2e27087f91c1095bf2090aec01ae00a +Subproject commit 56e2dc8f0433c3c73cd0888b543119c22b23a558 diff --git a/workspace/src/sdv_control/src/control/regulators/first_order/sdc1_vel_pid_node.cpp b/workspace/src/sdv_control/src/control/regulators/first_order/sdc1_vel_pid_node.cpp index 6e260eef..b2ac1bc6 100644 --- a/workspace/src/sdv_control/src/control/regulators/first_order/sdc1_vel_pid_node.cpp +++ b/workspace/src/sdv_control/src/control/regulators/first_order/sdc1_vel_pid_node.cpp @@ -11,7 +11,7 @@ #include #include "rclcpp/rclcpp.hpp" -#include "controllers/feedback_linearization/model_based_controllers/SDCs/regulators/vtec_sdc1_pid.hpp" +#include "controllers/feedback_linearization/model_based/SDCs/regulators/vtec_sdc1_pid.hpp" #include "utils/utils.hpp" #include "geometry_msgs/msg/accel.hpp" @@ -20,8 +20,6 @@ #include "std_msgs/msg/float32.hpp" #include "std_msgs/msg/u_int8.hpp" #include "std_msgs/msg/string.hpp" -#include "diagnostic_msgs/msg/diagnostic_status.hpp" -#include "diagnostic_msgs/msg/key_value.hpp" #include "sdv_msgs/msg/eta_pose.hpp" #include "vectornav_msgs/msg/ins_group.hpp" @@ -46,24 +44,29 @@ class CarControlNode : public rclcpp::Node float ki_; float kd_; uint8_t D_MAX_; - float U_MAX_{12800}; // MAX THROTTLE (pasarnos de esto no es bueno + double U_MAX_{12800}; // MAX THROTTLE (pasarnos de esto no es bueno // de acuerdo a sims con modelo parametrizado hasta step 95) + double U_MIN_{0}; + float vel_d_{0.0}; float vel_body_x_{0.0}; DOFControllerType_E controller_type_{LINEAR_DOF}; /* Model Params */ - std::unique_ptr model_; + std::shared_ptr car_; + std::unique_ptr pid_; std::vector init_pose_ = {0,0,0}; + PIDParameters pid_params_; rclcpp::TimerBase::SharedPtr timer_; - diagnostic_msgs::msg::DiagnosticStatus throttle_diag_; + geometry_msgs::msg::Accel accel_; + geometry_msgs::msg::Twist vel_; + sdv_msgs::msg::EtaPose pose_; /* Publishers */ rclcpp::Publisher::SharedPtr car_accel_; rclcpp::Publisher::SharedPtr car_vel_; rclcpp::Publisher::SharedPtr car_eta_pose_; - rclcpp::Publisher::SharedPtr throttle_diag_pub; rclcpp::Publisher::SharedPtr calc_throttle_; /* Subscribers */ @@ -84,21 +87,28 @@ class CarControlNode : public rclcpp::Node if(is_simulation_){ /* calculate Model States */ - model_->calculateModelParams(); - - model_->calculateStates(); + car_->calculateModelParams(); + car_->calculateStates(); - model_->updateNonLinearFunctions(); + pid_->updateNonLinearFunctions(); + pid_->calculateControlSignals(car_->velocities_(0), vel_d_, 0); + pid_->updateControlSignals(); - model_->calculateControlSignals(); - model_->updateControlSignals(); - model_->updateDBSignals(vel_d_); + car_->updateDBSignals(vel_d_); /* Publish Odometry */ - car_accel_->publish(model_->accelerations_); - car_vel_->publish(model_->velocities_); - car_eta_pose_->publish(model_->eta_pose_); - + accel_.linear.x = car_->accelerations_(0); + accel_.linear.y = car_->accelerations_(1); + + vel_.linear.x = car_->velocities_(0); + vel_.linear.y = car_->velocities_(1); + + pose_.x = car_->eta_pose_(0); + pose_.y = car_->eta_pose_(1); + + car_accel_->publish(accel_); + car_vel_->publish(vel_); + car_eta_pose_->publish(pose_); } else { if(drive_mode_ == "Automatic"){ @@ -107,16 +117,15 @@ class CarControlNode : public rclcpp::Node if(vel_msgs_received_){ /* calculate Model States */ - model_->calculateModelParams(); - - model_->calculateStates(); - - model_->updateNonLinearFunctions(); + car_->calculateModelParams(); + car_->calculateStates(); + pid_->updateNonLinearFunctions(); RCLCPP_INFO(this->get_logger(), "Vectornav vel received"); - model_->calculateControlSignals(vel_body_x_); - model_->updateControlSignals(); - model_->updateDBSignals(vel_d_); + pid_->calculateControlSignals(vel_body_x_, vel_d_, 0); + pid_->updateControlSignals(); + + car_->updateDBSignals(vel_d_); } else RCLCPP_INFO(this->get_logger(), "Waiting for vectornav"); } else { @@ -125,29 +134,20 @@ class CarControlNode : public rclcpp::Node } - /* Publish diagnostics */ - // TODO. Check for errors in throttle computation (maybe there are no real values) to updated diagnostics - // value.value = model_->u_(0); - // RCLCPP_INFO(this->get_logger(), "U: %f", model_->u_(0)); - diagnostic_msgs::msg::KeyValue value; - throttle_diag_.level = 0; - value.key = "throttle_signal"; - value.value = std::to_string(model_->D_); - throttle_diag_.values.push_back(value); - throttle_diag_pub->publish(throttle_diag_); + // RCLCPP_INFO(this->get_logger(), "U: %f", pid_->u_(0)); std_msgs::msg::UInt8 D; - D.data = model_->D_; + D.data = car_->D_; calc_throttle_->publish(D); - // model_->calculateCrosstrackError(x0,y0,x1,y1); + // pid_->calculateCrosstrackError(x0,y0,x1,y1); // std_msgs::msg::Float32 deltainfo; - // deltainfo.data = model_->delta_; + // deltainfo.data = pid_->delta_; // // car_steering_->publish(deltainfo); - // model_->updateSetpoint(4,0); + // pid_->updateSetpoint(4,0); // sdv_msgs::msg::ThrustControl u; - // u.tau_x = model_->u_; + // u.tau_x = pid_->u_; // car_force_->publish(u); } @@ -155,29 +155,28 @@ class CarControlNode : public rclcpp::Node void set_reference(const std_msgs::msg::Float32& msg) //const { vel_d_ = msg.data; - model_->updateCurrentReference(vel_d_, 0); } void save_velocity(const vectornav_msgs::msg::InsGroup::SharedPtr msg_in) //const { vel_body_x_ = msg_in->velbody.x; - // model_->updateCurrentReference(vel_body_x_, 0); + // pid_->updateCurrentReference(vel_body_x_, 0); vel_msgs_received_ = true; } void set_pitch(const vectornav_msgs::msg::CommonGroup::SharedPtr msg_in) //const { if(this->is_simulation_) - model_->setPitch(msg_in->yawpitchroll.y * M_PI / 180.0); + car_->setPitch(msg_in->yawpitchroll.y * M_PI / 180.0); } void set_steering(const std_msgs::msg::Float32& msg) //const { if(this->is_simulation_){ - model_->setSteering(msg.data); + car_->setSteering(msg.data); } else { // std::cout << std::to_string(msg.data * M_PI / 180.0) <setSteering(/*msg.data * M_PI / 180.0*/0.0); + car_->setSteering(/*msg.data * M_PI / 180.0*/0.0); } } @@ -190,6 +189,7 @@ class CarControlNode : public rclcpp::Node { auto_mode_ = msg.data; } + public: CarControlNode() : Node("sdc_control_node") { @@ -199,28 +199,35 @@ class CarControlNode : public rclcpp::Node //https://roboticsbackend.com/rclcpp-params-tutorial-get-set-ros2-params-with-cpp/ this->declare_parameter("is_simulation", rclcpp::PARAMETER_BOOL); this->declare_parameter("frequency", rclcpp::PARAMETER_INTEGER); // Super important to get parameters from launch files!! + this->declare_parameter("init_pose", rclcpp::PARAMETER_DOUBLE_ARRAY); + this->declare_parameter("Kp", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("Ki", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("Kd", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("U_MAX", rclcpp::PARAMETER_DOUBLE); + this->declare_parameter("U_MIN", rclcpp::PARAMETER_DOUBLE); this->declare_parameter("D_MAX", rclcpp::PARAMETER_INTEGER); - this->declare_parameter("init_pose", rclcpp::PARAMETER_DOUBLE_ARRAY); frequency = this->get_parameter("frequency").as_int(); is_simulation_ = this->get_parameter("is_simulation").as_bool(); + init_pose_ = this->get_parameter("init_pose").as_double_array(); + kp_ = this->get_parameter("Kp").as_double(); ki_ = this->get_parameter("Ki").as_double(); kd_ = this->get_parameter("Kd").as_double(); - this->get_parameter_or("D_MAX", D_MAX_, static_cast(180)); - init_pose_ = this->get_parameter("init_pose").as_double_array(); - - // std::cout << "Freq = " << frequency << std::endl; - // std::cout << "kp = " << kp_ << std::endl; - // std::cout << "ki = " << ki_ << std::endl; - // std::cout << "kd = " << kd_ << std::endl; - // std::cout << "D_MAX = " << static_cast(D_MAX_) << std::endl; + this->get_parameter_or("U_MAX", U_MAX_, static_cast(12800)); + this->get_parameter_or("U_Min", U_MIN_, static_cast(0)); + this->get_parameter_or("D_MAX", D_MAX_, static_cast(90)); sample_time_ = 1.0 / static_cast(frequency); + pid_params_.kP = kp_; + pid_params_.kI = ki_; + pid_params_.kD = kd_; + pid_params_.kUMax = U_MAX_; + pid_params_.kUMin = U_MIN_; + pid_params_.kDt = sample_time_; + /* Publishers */ if(is_simulation_){ car_accel_ = this->create_publisher("/sdc_simulation/dynamic_model/accel", 10); @@ -228,7 +235,6 @@ class CarControlNode : public rclcpp::Node car_eta_pose_ = this->create_publisher("/sdc_simulation/dynamic_model/eta_pose", 10); } calc_throttle_ = this->create_publisher("/sdc_control/control_signal/D",10); - throttle_diag_pub = this->create_publisher("/diagnostics",10); // car_force_ = this->create_publisher("/sdc_control/sdc_control_node/force",1); /* Subscribers */ @@ -257,24 +263,20 @@ class CarControlNode : public rclcpp::Node auto_mode_sub_ = this->create_subscription("/sdv/xbox_controller/auto_mode", 1, std::bind(&CarControlNode::set_auto_mode, this, std::placeholders::_1)); - throttle_diag_.name = "Throttle command (D)"; - throttle_diag_.message = "Integer in the range of [0, 255] for motor controller"; - throttle_diag_.hardware_id = "Throttle"; - timer_ = this->create_wall_timer( std::chrono::milliseconds(1000 / frequency), std::bind(&CarControlNode::timer_callback, this)); } - ~CarControlNode(){model_.reset();} + ~CarControlNode(){/*pid_.reset();*/} void configure(){ - model_ = std::make_unique(sample_time_, kp_, ki_, kd_, - U_MAX_, controller_type_, D_MAX_); + car_ = std::make_shared(sample_time_, D_MAX_); + pid_ = std::make_unique(pid_params_, car_); - std::vector init_pose = {static_cast(init_pose_[0]), - static_cast(init_pose_[1]), - static_cast(init_pose_[2])}; - model_->setInitPose(init_pose); + Eigen::Vector3f init_pose = {static_cast(init_pose_[0]), + static_cast(init_pose_[1]), + static_cast(init_pose_[2])}; + car_->setInitPose(init_pose); } }; diff --git a/workspace/src/sdv_control/src/model/car_tf2_broadcast_node.cpp b/workspace/src/sdv_control/src/model/car_tf2_broadcast_node.cpp new file mode 100644 index 00000000..bffe6907 --- /dev/null +++ b/workspace/src/sdv_control/src/model/car_tf2_broadcast_node.cpp @@ -0,0 +1,86 @@ +/** ---------------------------------------------------------------------------- + * @file: car_tf2_broadcast_node.cpp + * @date: August 12, 2023 + * @author: Sebas Mtz + * @author: Edison Altamirano + * + * @brief: tf2 broadcast node for RViz + * ----------------------------------------------------------------------------- + **/ + +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "../simulation/sdc1_broadcaster.hpp" + +#include "nav_msgs/msg/path.hpp" + +#include "sdv_msgs/msg/eta_pose.hpp" + +using namespace std::chrono_literals; + +class CarTf2Broadcast : public rclcpp::Node +{ + private: + int frequency_; + sdv_msgs::msg::EtaPose car_pose_; + + rclcpp::TimerBase::SharedPtr timer_; + std::unique_ptr tf_broadcaster_; + + rclcpp::Publisher::SharedPtr car_path_; + + rclcpp::Subscription::SharedPtr car_eta_pose_; + + std::string parent_frame_; + std::string child_frame_; + + void timer_callback() + { + tf_broadcaster_->broadcastTransform(car_pose_); + car_path_->publish(tf_broadcaster_->path_); + } + + void set_sim_pose(const sdv_msgs::msg::EtaPose& msg) + { + car_pose_.x = msg.x; + car_pose_.y = msg.y; + car_pose_.psi = msg.psi; + } + + public: + CarTf2Broadcast() : Node("car_t2_broadcast_node") + { + this->declare_parameter("frequency", rclcpp::PARAMETER_INTEGER); // Super important to get parameters from launch files!! + + this->get_parameter_or("frequency", frequency_, 100); + + car_path_ = this->create_publisher("/sdc_simulation/car_tf_broadcast/car_path", 10); + + car_eta_pose_ = this->create_subscription("/sdc_simulation/dynamic_model/eta_pose", + 1, std::bind(&CarTf2Broadcast::set_sim_pose, this, std::placeholders::_1)); + + timer_ = this->create_wall_timer( + std::chrono::milliseconds(1000 / frequency_), + std::bind(&CarTf2Broadcast::timer_callback, this)); + } + + ~CarTf2Broadcast(){tf_broadcaster_.reset();} + + void configure() + { + tf_broadcaster_ = std::make_unique(shared_from_this()); + } + +}; + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + auto tf2_node = std::make_shared(); + tf2_node->configure(); + rclcpp::spin(tf2_node->get_node_base_interface()); + rclcpp::shutdown(); + return 0; +} \ No newline at end of file diff --git a/workspace/src/sdv_control/src/simulation/sdc1_broadcaster.cpp b/workspace/src/sdv_control/src/simulation/sdc1_broadcaster.cpp new file mode 100644 index 00000000..974ece2c --- /dev/null +++ b/workspace/src/sdv_control/src/simulation/sdc1_broadcaster.cpp @@ -0,0 +1,70 @@ +/** ---------------------------------------------------------------------------- + * @file: sdc1_broadcaster.hpp + * @date: August 23, 2023 + * @author: Sebastian Martinez + * @email: sebas.martp@gmail.com + * + * @brief: TF2 broadcaster class to publish transformations for the odom and + * base link frames of the SDC1. + * Frame Conventions: + * - ODOM: (NED) + - x (front) + - y (right) + - z (down) + * - BASE_LINK: + - x (front) + - y (right) + - z (down) + * ----------------------------------------------------------------------------- + **/ +#include "sdc1_broadcaster.hpp" + +SDC1Broadcaster::SDC1Broadcaster(rclcpp::Node::SharedPtr node):TF2Broadcaster(node, "odom", "base_link") +{} + +SDC1Broadcaster::~SDC1Broadcaster(){} + +void SDC1Broadcaster::broadcastTransform(const sdv_msgs::msg::EtaPose& _pose) +{ + // The definition of this method considers the same coordinate frame convention + // for both the parent and child frame + + // To visualize correctly in RViz in the desired reference frame, just make sure to define correctly + // your tfs and select the proper fixed frame (ex: map --> odom(ned) --> base_link, with static tf between map and odom) + // The fixed frame would be map, and by displaying the tfs, viewing odom, measuremnts would make sense + + // MAKE SURE YOU KNOW YOUR FRAMES!!! + + tf_stmpd_.header.stamp = node_->now(); + tf_stmpd_.header.frame_id = this->parent_frame; + tf_stmpd_.child_frame_id = this->child_frame; + tf_stmpd_.transform.translation.x = _pose.x; + tf_stmpd_.transform.translation.y = _pose.y; + tf_stmpd_.transform.translation.z = _pose.z; + + tf2::Quaternion q; + q.setRPY(_pose.phi, _pose.theta, _pose.psi); + q.normalize(); + tf_stmpd_.transform.rotation.x = q.x(); + tf_stmpd_.transform.rotation.y = q.y(); + tf_stmpd_.transform.rotation.z = q.z(); + tf_stmpd_.transform.rotation.w = q.w(); + + this->br_->sendTransform(tf_stmpd_); + + geometry_msgs::msg::PoseStamped pose; + + pose.header.stamp = node_->now(); + pose.header.frame_id = this->parent_frame; + pose.pose.position.x = _pose.x; + pose.pose.position.y = _pose.y; + pose.pose.position.z = _pose.z; + pose.pose.orientation.x = q.x(); + pose.pose.orientation.y = q.y(); + pose.pose.orientation.z = q.z(); + pose.pose.orientation.w = q.w(); + + this->path_.header.stamp = node_->now(); + this->path_.header.frame_id = this->parent_frame; + this->path_.poses.push_back(pose); +} \ No newline at end of file diff --git a/workspace/src/sdv_control/src/simulation/sdc1_broadcaster.hpp b/workspace/src/sdv_control/src/simulation/sdc1_broadcaster.hpp new file mode 100644 index 00000000..77bb88b6 --- /dev/null +++ b/workspace/src/sdv_control/src/simulation/sdc1_broadcaster.hpp @@ -0,0 +1,37 @@ +/** ---------------------------------------------------------------------------- + * @file: sdc1_broadcaster.hpp + * @date: August 23, 2023 + * @author: Sebastian Martinez + * @email: sebas.martp@gmail.com + * + * @brief: TF2 broadcaster class to publish transformations for the odom and + * base link frames of the SDC1. + * Frame Conventions: + * - ODOM: + - x (front) + - y (right) + - z (down) + * - BASE_LINK: + - x (front) + - y (right) + - z (down) + * ----------------------------------------------------------------------------- + **/ + +#ifndef __SDC1_BROADCASTER_H__ +#define __SDC1_BROADCASTER_H__ + +#include "rclcpp/rclcpp.hpp" + +#include "tf2_6dof_broadcaster_ros2.hpp" + +class SDC1Broadcaster : public TF2Broadcaster +{ + public: + SDC1Broadcaster(rclcpp::Node::SharedPtr node); + ~SDC1Broadcaster(); + + void broadcastTransform(const sdv_msgs::msg::EtaPose& msg); +}; + +#endif \ No newline at end of file diff --git a/workspace/src/sdv_control/src/simulation/tf2_6dof_broadcaster_ros2.cpp b/workspace/src/sdv_control/src/simulation/tf2_6dof_broadcaster_ros2.cpp new file mode 100644 index 00000000..b6737c23 --- /dev/null +++ b/workspace/src/sdv_control/src/simulation/tf2_6dof_broadcaster_ros2.cpp @@ -0,0 +1,31 @@ +/** ---------------------------------------------------------------------------- + * @file: tf2_6dof_broadcaster.cpp + * @date: August 10, 2023 + * @author: Sebastian Martinez + * @email: sebas.martp@gmail.com + * + * @brief: Base TF2 broadcaster class to publish transformations + * from a parent frame to a child frame. + * The broadcastTransform() method must be defined in derived classes + * as the transform is strictly dependent of the frame conventions used. + * ----------------------------------------------------------------------------- + **/ + +#include "tf2_6dof_broadcaster_ros2.hpp" + +TF2Broadcaster::TF2Broadcaster(rclcpp::Node::SharedPtr node,const std::string& _parent, const std::string& _child): + node_(node), parent_frame(_parent), child_frame(_child) +{ + br_ = std::make_unique(node_); +} + +TF2Broadcaster::~TF2Broadcaster(){} + +void TF2Broadcaster::broadcastTransform(const sdv_msgs::msg::EtaPose& _pose) +{ + // To visualize correctly in RViz in the desired reference frame, just make sure to define correctly + // your tfs and select the proper fixed frame (ex: map --> odom(ned) --> base_link) + // The fixed frame would be map, and by displaying the tfs, viewing odom, measuremnts would make sense + + // MAKE SURE YOU KNOW YOUR FRAMES!!! +} \ No newline at end of file diff --git a/workspace/src/sdv_control/src/simulation/tf2_6dof_broadcaster_ros2.hpp b/workspace/src/sdv_control/src/simulation/tf2_6dof_broadcaster_ros2.hpp new file mode 100644 index 00000000..27a15623 --- /dev/null +++ b/workspace/src/sdv_control/src/simulation/tf2_6dof_broadcaster_ros2.hpp @@ -0,0 +1,60 @@ +/** ---------------------------------------------------------------------------- + * @file: tf2_6dof_broadcaster.hpp + * @date: August 10, 2023 + * @author: Sebastian Martinez + * @email: sebas.martp@gmail.com + * + * @brief: Base TF2 broadcaster class to publish transformations + * from a parent frame to a child frame. + * The broadcastTransform() method must be defined in derived classes + * as the transform is strictly dependent of the frame conventions used. + * ----------------------------------------------------------------------------- + **/ + +#ifndef __TF2_6DOF_BROADCASTER2_H__ +#define __TF2_6DOF_BROADCASTER2_H__ + +#include +#include +#include "rclcpp/rclcpp.hpp" + +#include "tf2/LinearMath/Quaternion.h" +#include "tf2_ros/transform_broadcaster.h" + +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "nav_msgs/msg/path.hpp" +#include "sdv_msgs/msg/eta_pose.hpp" + +class TF2Broadcaster +{ + public: + nav_msgs::msg::Path path_; + + /* Class methods */ + // The broadcastTransform() method must be defined in derived classes + // as the transform is strictly dependent of the frame conventions used. + + // To visualize correctly in RViz in the desired reference frame, just make sure to define correctly + // your tfs and select the proper fixed frame (ex: map --> odom(ned) --> base_link) + // The fixed frame would be map, and by displaying the tfs, viewing odom, measuremnts would make sense + + // MAKE SURE YOU KNOW YOUR FRAMES!!! + virtual void broadcastTransform(const sdv_msgs::msg::EtaPose& msg); + + protected: + rclcpp::Node::SharedPtr node_; + std::string parent_frame; + std::string child_frame; + + geometry_msgs::msg::TransformStamped tf_stmpd_; + + std::unique_ptr br_; + + TF2Broadcaster(rclcpp::Node::SharedPtr node,const std::string& _parent, const std::string& _child); + ~TF2Broadcaster(); + +}; + +#endif \ No newline at end of file diff --git a/workspace/src/sdv_localization/sdv_localization/CMakeLists.txt b/workspace/src/sdv_localization/sdv_localization/CMakeLists.txt index 4b69bffe..52e78ec0 100644 --- a/workspace/src/sdv_localization/sdv_localization/CMakeLists.txt +++ b/workspace/src/sdv_localization/sdv_localization/CMakeLists.txt @@ -28,7 +28,7 @@ find_package(nav_msgs REQUIRED) find_package(std_srvs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) find_package(rosidl_default_generators REQUIRED) -find_package(sbg_driver REQUIRED) +# find_package(sbg_driver REQUIRED) #find_package(sdv_localization_msgs REQUIRED) # find_package(ament_cmake_python REQUIRED) @@ -61,12 +61,12 @@ ament_target_dependencies(sbg_processing rclcpp sensor_msgs tf2_geometry_msgs na ament_target_dependencies(temp_tf_odom_map rclcpp sensor_msgs vectornav_msgs tf2_geometry_msgs nav_msgs) install(TARGETS - vn_processing - sbg_processing + # vn_processing + # sbg_processing # diagnostics # vslam_repub # isaac_awake - temp_tf_odom_map + # temp_tf_odom_map DESTINATION lib/${PROJECT_NAME} ) diff --git a/workspace/src/sdv_localization/sdv_localization/launch/sdv_localization.launch.py b/workspace/src/sdv_localization/sdv_localization/launch/sdv_localization.launch.py index 91585f06..be004362 100644 --- a/workspace/src/sdv_localization/sdv_localization/launch/sdv_localization.launch.py +++ b/workspace/src/sdv_localization/sdv_localization/launch/sdv_localization.launch.py @@ -102,7 +102,10 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', name="tf_odom_base_link", - arguments = ['0', '0', '0', '0', '0', '-3.14159', 'map', 'odom'], # x y z yaw pitch roll + arguments = [ + '--x', '0', '--y', '0', '--z', '0', + '--roll', '0', '--pitch', '0', '--yaw', '-3.14159', + '--frame-id', 'map', '--child-frame-id', 'odom'], # condition=UnlessCondition(LaunchConfiguration('is_simulation')) condition=IfCondition( PythonExpression([ @@ -120,7 +123,10 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', name="tf_map_to_odom", - arguments = ['0', '0', '0', '0', '0', '0', 'map', 'odom'], + arguments = [ + '--x', '0', '--y', '0', '--z', '0', + '--roll', '0', '--pitch', '0', '--yaw', '0', + '--frame-id', 'map', '--child-frame-id', 'odom'], # condition=UnlessCondition(LaunchConfiguration('is_simulation')) condition=IfCondition( PythonExpression([ @@ -137,7 +143,10 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', name="tf_base_link_to_vectornav", - arguments = ['0', '0', '1.9', '0', '0.0', '0.0', 'base_link', 'vectornav'], + arguments = [ + '--x', '0', '--y', '0', '--z', '1.9', + '--roll', '0', '--pitch', '0', '--yaw', '0', + '--frame-id', 'base_link', '--child-frame-id', 'vectornav'], condition=UnlessCondition(LaunchConfiguration('is_simulation')) ) @@ -153,7 +162,10 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', name="tf_base_link_to_velodyne", - arguments = ['0.45', '0', '2.25', '-0.05', '0.0', '0.0', 'base_link', 'velodyne'], #-0.05 y + arguments = [ + '--x', '0.45', '--y', '0', '--z', '2.25', + '--roll', '0', '--pitch', '0', '--yaw', '-0.05', + '--frame-id', 'base_link', '--child-frame-id', 'velodyne'], #-0.05 y condition=UnlessCondition(LaunchConfiguration('is_simulation')) ) @@ -162,7 +174,10 @@ def generate_launch_description(): package='tf2_ros', executable='static_transform_publisher', name="tf_map_to_scan", - arguments = ['0', '0', '0', '0', '0', '0', 'map', 'scan'], + arguments = [ + '--x', '0', '--y', '0', '--z', '0', + '--roll', '0', '--pitch', '0', '--yaw', '0', + '--frame-id', 'map', '--child-frame-id', 'scan'], # condition=UnlessCondition(LaunchConfiguration('is_simulation')) condition=IfCondition( PythonExpression([