Implement reset functionality for pnc_sim#1
Open
michaelwestern1 wants to merge 2 commits into
Open
Conversation
obaidmm
requested changes
Jan 19, 2026
obaidmm
left a comment
obaidmm
left a comment
Contributor
There was a problem hiding this comment.
please resolve conflicts in sim_node.hpp
michaelwestern1
force-pushed
the
pc-87-reset-update
branch
from
67e9a4f to
98f6b97
Compare
alyashour
requested changes
Feb 14, 2026
Comment on lines
+17
to
+19
| #include "ap1_msgs/msg/motor_power_stamped.hpp" | ||
| #include "ap1_msgs/msg/turn_angle_stamped.hpp" | ||
| #include "ap1_msgs/msg/vehicle_speed_stamped.hpp" |
Member
There was a problem hiding this comment.
These types do not exist anymore. Use FloatStamped.
Comment on lines
-64
to
-72
| /** | ||
|
|
||
| mass_kg = inertia which makes acceleration slower, car feels heavier | ||
| max_force_n = engine power, raises acceleration and top speed | ||
| drag_coeff = air/friction resistance, reduces top speed and causes quicker slow-down when | ||
| throttle drops wheelbase_m = distance between axles, influences turning radius — longer = wider | ||
| turns max_steer_rad = steering limit, allows tighter turns if larger | ||
|
|
||
| */ |
Comment on lines
-89
to
-90
| float yaw; // radians | ||
| float v_mps; // forward speed |
Comment on lines
-131
to
162
| // I hope this works but very very questionable | ||
|
|
||
| // Basic longitudinal + kinematic bicycle dynamics | ||
| void step_dynamics(float dt) | ||
| { | ||
| // Longitudinal force from engine | ||
| float engine_force = current_throttle_ * cfg_.max_force_n; | ||
|
|
||
| // Very crude drag, proportional to speed | ||
| float drag_force = cfg_.drag_coeff * state_.v_mps; | ||
|
|
||
| // Net force and acceleration | ||
| float net_force = engine_force - drag_force; | ||
| float accel = net_force / cfg_.mass_kg; | ||
| float accel = (engine_force - drag_force) / cfg_.mass_kg; | ||
|
|
||
| // Integrate velocity | ||
| state_.v_mps += accel * dt; | ||
| if (state_.v_mps < 0.0f) | ||
| { | ||
| state_.v_mps = 0.0f; | ||
| } | ||
| state_.v_mps = std::max(0.0f, state_.v_mps); | ||
|
|
||
| // Kinematic turning | ||
| float beta = std::tan(current_steer_rad_); | ||
| float yaw_rate = (state_.v_mps / cfg_.wheelbase_m) * beta; | ||
|
|
||
| state_.yaw += yaw_rate * dt; | ||
|
|
||
| // Integrate position | ||
| state_.x_m += state_.v_mps * std::cos(state_.yaw) * dt; | ||
| state_.y_m += state_.v_mps * std::sin(state_.yaw) * dt; | ||
| } |
Member
There was a problem hiding this comment.
This PR is for reset state, no need to update this func.
Comment on lines
-166
to
-172
| // Speed to Planner | ||
| ap1_msgs::msg::FloatStamped speed_msg; | ||
| speed_msg.value = state_.v_mps; | ||
| speed_pub_->publish(speed_msg); | ||
|
|
||
| // Position to whoever wants it (need to add a "subscriber" in Planner, currently connected | ||
| // to control) |
Comment on lines
179
to
-188
|
|
||
| // Config and state | ||
| CarConfig cfg_; | ||
| CarState state_; | ||
|
|
||
| // Inputs from Control | ||
| float current_throttle_; | ||
| float current_steer_rad_; | ||
|
|
||
| // ROS stuff |
Comment on lines
-16
to
-45
| add_executable(pnc_sim_node src/main.cpp) | ||
| target_include_directories(pnc_sim_node PUBLIC | ||
| $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> | ||
| $<INSTALL_INTERFACE:include/${PROJECT_NAME}>) | ||
| target_compile_features(pnc_sim_node PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 | ||
| ament_target_dependencies( | ||
| pnc_sim_node | ||
| "rclcpp" | ||
| "std_msgs" | ||
| "geometry_msgs" | ||
| "nav_msgs" | ||
| "ap1_msgs" | ||
| # -------------------------------------------------- | ||
| # INCLUDE DIRECTORIES | ||
| # -------------------------------------------------- | ||
| include_directories( | ||
| include | ||
| ) | ||
|
|
||
| install(TARGETS | ||
| pnc_sim_node | ||
| DESTINATION lib/${PROJECT_NAME} | ||
| # -------------------------------------------------- | ||
| # BUILD EXECUTABLE | ||
| # -------------------------------------------------- | ||
| add_executable(pnc_sim_node | ||
| src/main.cpp | ||
| src/simple_vehicle_sim_node.cpp | ||
| ) | ||
|
|
||
| if(BUILD_TESTING) | ||
| find_package(ament_lint_auto REQUIRED) | ||
| # the following line skips the linter which checks for copyrights | ||
| # comment the line when a copyright and license is added to all source files | ||
| set(ament_cmake_copyright_FOUND TRUE) | ||
| # the following line skips cpplint (only works in a git repo) | ||
| # comment the line when this package is in a git repo and when | ||
| # a copyright and license is added to all source files | ||
| set(ament_cmake_cpplint_FOUND TRUE) | ||
| ament_lint_auto_find_test_dependencies() | ||
| endif() |
Member
There was a problem hiding this comment.
Are these necessary? Stay on topic of the PR. You can just linklib yaml-cpp to get that working no need to restructure the whole thing
| @@ -0,0 +1 @@ | |||
| rate_hz: 50 | |||
Member
There was a problem hiding this comment.
Where is this used in the code?
Member
Also edit this to include details like what the PR is for |
michaelwestern1
force-pushed
the
pc-87-reset-update
branch
from
98f6b97 to
798f604
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
3 participants
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
Merge Request / Pull Request
Summary of Changes
Briefly describe what this MR/PR does and which issue/task it addresses.
Type of Change
Checklist (to be completed before review)
Related Issue / Task
Notes / Additional Context
Any context for reviewers (limitations, known issues, future work)