Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat: add simple planning simulator package #5

Merged
Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
64 commits
Select commit Hold shift + click to select a range
8e97427
release v0.4.0
mitsudome-r Sep 18, 2020
bbcddb4
remove ROS1 packages temporarily
mitsudome-r Sep 29, 2020
edc7b36
add sample ros2 packages
mitsudome-r Sep 29, 2020
5fac7eb
add COLCON_IGNORE to ros1 packages
mitsudome-r Oct 8, 2020
c41b710
Fix simple planning simulator (#26)
TakaHoribe Oct 13, 2020
e14ecad
[simple_planning_simulator] add rostopic relay in launch file (#117)
mitsudome-r Nov 24, 2020
7406784
Adjust copyright notice on 532 out of 699 source files (#143)
nnmm Dec 3, 2020
7aa3365
Use quotes for includes where appropriate (#144)
nnmm Dec 7, 2020
c2ecb34
Run uncrustify on the entire Pilot.Auto codebase (#151)
nnmm Dec 8, 2020
ffab21d
reduce terminal ouput for better error message visibility (#200)
mitsudome-r Dec 23, 2020
fc3691f
Use trajectory for z position source (#243)
wep21 Dec 24, 2020
a97a775
Ros2 v0.8.0 engage (#342)
wep21 Feb 17, 2021
4dcfe97
Ros2 v0.8.0 fix packages (#351)
tkimura4 Feb 24, 2021
bdd1791
Rename ROS-related .yaml to .param.yaml (#352)
kenji-miyake Feb 24, 2021
bf134df
Fix typo in simulator module (#439)
kmiya Mar 16, 2021
b222d75
add use_sim-time option (#454)
tkimura4 Mar 26, 2021
4ef120c
Format launch files (#1219)
kenji-miyake Mar 30, 2021
0ccd6f1
Fix rolling build errors (#1225)
kenji-miyake Apr 5, 2021
9c2f176
Sync public repo (#1228)
mitsudome-r Apr 5, 2021
2e3c97d
Remove use_sim_time for set_parameter (#1260)
wep21 Apr 26, 2021
0289c8f
Refactor vehicle info util (#1305)
kenji-miyake May 11, 2021
c5f4978
Fix lint errors (#1378)
kenji-miyake May 25, 2021
52b35cf
Add pre-commit (#1560)
KeisukeShima Jul 19, 2021
04ccb95
Add markdownlint and prettier (#1661)
kenji-miyake Jul 20, 2021
5b7f460
add cov pub in psim (#1732)
k0suke-murakami Jul 30, 2021
f74b212
Fix -Wunused-parameter (#1836)
kenji-miyake Aug 14, 2021
c506ed7
fix some typos (#1941)
h-ohta Aug 25, 2021
4f99ca0
Add autoware api (#1979)
isamu-takagi Aug 31, 2021
7a3670e
add sort-package-xml hook in pre-commit (#1881)
KeisukeShima Sep 9, 2021
467c390
Feature/add ideal accel model interface (#1894)
mkuri Oct 21, 2021
7a8aa72
Change formatter to clang-format and black (#2332)
kenji-miyake Nov 2, 2021
b96e68b
Add COLCON_IGNORE (#500)
kenji-miyake Nov 4, 2021
789899e
Back port .auto control packages (#571)
TakaHoribe Nov 11, 2021
059f0e1
[simple planning simulator]change type of msg (#590)
tkimura4 Nov 12, 2021
82666d7
[autoware_vehicle_rviz_plugin/route_handler/simple_planning_simulator…
tkimura4 Nov 12, 2021
ee7955b
update to support velocity report header (#655)
TakaHoribe Nov 15, 2021
cc7525d
adapt to actuation cmd/status as control msg (#646)
taikitanaka3 Nov 16, 2021
b47409b
FIx vehicle status topic name/type (#658)
tkimura4 Nov 16, 2021
9068bd1
fix topic name (#674)
TakaHoribe Nov 17, 2021
4845cd6
Fix psim param path (#696)
isamu-takagi Nov 18, 2021
40add7f
Fix/psim topics emergency handler awapi (#702)
taikitanaka3 Nov 18, 2021
bc9c8c5
Auto/add turn indicators and hazards (#717)
kyoichi-sugahara Nov 19, 2021
336c5f9
[simple_planning_simulator]fix bug (#727)
tkimura4 Nov 24, 2021
530a622
fix gear process in sim (#728)
tkimura4 Nov 24, 2021
8168271
Fix for integration test (#732)
isamu-takagi Nov 25, 2021
69ad7af
Simple planning simulator update for latest develop (#735)
TakaHoribe Nov 26, 2021
4acd06b
Fix acceleration for reverse (#737)
rej55 Nov 29, 2021
764a4e7
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
1222-takeshi Dec 2, 2021
7731f62
ci(pre-commit): autofix
pre-commit-ci[bot] Dec 2, 2021
bf2fc6b
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
1222-takeshi Dec 2, 2021
373f7a4
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
1222-takeshi Dec 3, 2021
a6f18eb
remove tests
tkimura4 Dec 6, 2021
3c67709
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
tkimura4 Dec 6, 2021
17b40e0
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
tkimura4 Dec 6, 2021
e1abab4
Revert "ci(pre-commit): autofix"
tkimura4 Dec 6, 2021
2306e18
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
tkimura4 Dec 6, 2021
e277c4c
fix format
tkimura4 Dec 6, 2021
83cf537
Merge branch '1-add-simple-planning-simulator' of github.com:tkimura4…
tkimura4 Dec 6, 2021
72d50cf
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
tkimura4 Dec 6, 2021
7c1f7c1
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
tkimura4 Dec 6, 2021
9a8ae1e
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
tkimura4 Dec 6, 2021
d947eee
fix autoware_api_utils
tkimura4 Dec 6, 2021
68b5ded
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
tkimura4 Dec 6, 2021
6fb3e65
Merge branch 'tier4/proposal' into 1-add-simple-planning-simulator
tkimura4 Dec 6, 2021
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Prev Previous commit
Next Next commit
Run uncrustify on the entire Pilot.Auto codebase (#151)
* Run uncrustify on the entire Pilot.Auto codebase

* Exclude open PRs
  • Loading branch information
nnmm authored and tkimura4 committed Nov 30, 2021
commit c2ecb3441e07ab0f7adf0dc5fb1806776c5ac2b8
Original file line number Diff line number Diff line change
Expand Up @@ -121,7 +121,8 @@ class Simulator : public rclcpp::Node
std::shared_ptr<rclcpp::Time> prev_update_time_ptr_; //!< @brief previously updated time

/* vehicle model */
enum class VehicleModelType {
enum class VehicleModelType
{
IDEAL_TWIST = 0,
IDEAL_STEER = 1,
DELAY_TWIST = 2,
Expand All @@ -137,15 +138,15 @@ class Simulator : public rclcpp::Node
/* to generate measurement noise */
std::shared_ptr<std::mt19937> rand_engine_ptr_; //!< @brief random engine for measurement noise
std::shared_ptr<std::normal_distribution<>>
pos_norm_dist_ptr_; //!< @brief Gaussian noise for position
pos_norm_dist_ptr_; //!< @brief Gaussian noise for position
std::shared_ptr<std::normal_distribution<>>
vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity
vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity
std::shared_ptr<std::normal_distribution<>>
rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw
rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw
std::shared_ptr<std::normal_distribution<>>
angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity
angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity
std::shared_ptr<std::normal_distribution<>>
steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle
steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle

/**
* @brief set current_vehicle_cmd_ptr_ with received message
Expand All @@ -165,7 +166,8 @@ class Simulator : public rclcpp::Node
/**
* @brief set initial pose for simulation with received message
*/
void callbackInitialPoseWithCov(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);
void callbackInitialPoseWithCov(
const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg);

/**
* @brief set initial pose with received message
Expand Down Expand Up @@ -202,7 +204,9 @@ class Simulator : public rclcpp::Node
* @param [in] pose initial position and orientation
* @param [in] twist initial velocity and angular velocity
*/
void setInitialState(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist);
void setInitialState(
const geometry_msgs::msg::Pose & pose,
const geometry_msgs::msg::Twist & twist);

/**
* @brief set initial state of simulated vehicle with pose transformation based on frame_id
Expand All @@ -218,14 +222,17 @@ class Simulator : public rclcpp::Node
* @param [in] twist initial velocity and angular velocity
*/
void setInitialStateWithPoseTransform(
const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const geometry_msgs::msg::Twist & twist);
const geometry_msgs::msg::PoseWithCovarianceStamped & pose,
const geometry_msgs::msg::Twist & twist);

/**
* @brief publish pose and twist
* @param [in] pose pose to be published
* @param [in] twist twist to be published
*/
void publishPoseTwist(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist);
void publishPoseTwist(
const geometry_msgs::msg::Pose & pose,
const geometry_msgs::msg::Twist & twist);

/**
* @brief publish tf
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -50,14 +50,16 @@ class SimModelConstantAccelTwist : public SimModelInterface
~SimModelConstantAccelTwist() = default;

private:
enum IDX {
enum IDX
{
X = 0,
Y,
YAW,
VX,
WZ,
};
enum IDX_U {
enum IDX_U
{
VX_DES = 0,
WZ_DES,
};
Expand Down Expand Up @@ -111,4 +113,4 @@ class SimModelConstantAccelTwist : public SimModelInterface
Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override;
};

#endif
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -47,12 +47,14 @@ class SimModelIdealTwist : public SimModelInterface
~SimModelIdealTwist() = default;

private:
enum IDX {
enum IDX
{
X = 0,
Y,
YAW,
};
enum IDX_U {
enum IDX_U
{
VX_DES = 0,
WZ_DES,
};
Expand Down Expand Up @@ -120,12 +122,14 @@ class SimModelIdealSteer : public SimModelInterface
~SimModelIdealSteer() = default;

private:
enum IDX {
enum IDX
{
X = 0,
Y,
YAW,
};
enum IDX_U {
enum IDX_U
{
VX_DES = 0,
STEER_DES,
};
Expand Down Expand Up @@ -195,13 +199,15 @@ class SimModelIdealAccel : public SimModelInterface
~SimModelIdealAccel() = default;

private:
enum IDX {
enum IDX
{
X = 0,
Y,
YAW,
VX,
};
enum IDX_U {
enum IDX_U
{
AX_DES = 0,
STEER_DES,
};
Expand Down Expand Up @@ -252,4 +258,4 @@ class SimModelIdealAccel : public SimModelInterface
Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override;
};

#endif
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -64,14 +64,16 @@ class SimModelTimeDelayTwist : public SimModelInterface
private:
const double MIN_TIME_CONSTANT; //!< @brief minimum time constant

enum IDX {
enum IDX
{
X = 0,
Y,
YAW,
VX,
WZ,
};
enum IDX_U {
enum IDX_U
{
VX_DES = 0,
WZ_DES,
};
Expand Down Expand Up @@ -170,14 +172,16 @@ class SimModelTimeDelaySteer : public SimModelInterface
private:
const double MIN_TIME_CONSTANT; //!< @brief minimum time constant

enum IDX {
enum IDX
{
X = 0,
Y,
YAW,
VX,
STEER,
};
enum IDX_U {
enum IDX_U
{
VX_DES = 0,
STEER_DES,
};
Expand Down Expand Up @@ -276,15 +280,17 @@ class SimModelTimeDelaySteerAccel : public SimModelInterface
private:
const double MIN_TIME_CONSTANT; //!< @brief minimum time constant

enum IDX {
enum IDX
{
X = 0,
Y,
YAW,
VX,
STEER,
ACCX,
};
enum IDX_U {
enum IDX_U
{
ACCX_DES = 0,
STEER_DES,
DRIVE_SHIFT,
Expand Down Expand Up @@ -354,4 +360,4 @@ class SimModelTimeDelaySteerAccel : public SimModelInterface
Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override;
};

#endif
#endif
Original file line number Diff line number Diff line change
Expand Up @@ -282,7 +282,8 @@ void Simulator::timerCallbackSimulation()
if (
cmd == autoware_vehicle_msgs::msg::TurnSignal::LEFT ||
cmd == autoware_vehicle_msgs::msg::TurnSignal::RIGHT ||
cmd == autoware_vehicle_msgs::msg::TurnSignal::HAZARD) {
cmd == autoware_vehicle_msgs::msg::TurnSignal::HAZARD)
{
turn_signal_msg.data = cmd;
}
}
Expand All @@ -291,9 +292,9 @@ void Simulator::timerCallbackSimulation()
autoware_vehicle_msgs::msg::ShiftStamped shift_msg;
shift_msg.header.frame_id = simulation_frame_id_;
shift_msg.header.stamp = get_clock()->now();
shift_msg.shift.data = current_twist_.linear.x >= 0.0
? autoware_vehicle_msgs::msg::Shift::DRIVE
: autoware_vehicle_msgs::msg::Shift::REVERSE;
shift_msg.shift.data = current_twist_.linear.x >= 0.0 ?
autoware_vehicle_msgs::msg::Shift::DRIVE :
autoware_vehicle_msgs::msg::Shift::REVERSE;
pub_shift_->publish(shift_msg);

/* publish control mode */
Expand All @@ -307,7 +308,8 @@ void Simulator::callbackVehicleCmd(

if (
vehicle_model_type_ == VehicleModelType::IDEAL_STEER ||
vehicle_model_type_ == VehicleModelType::DELAY_STEER) {
vehicle_model_type_ == VehicleModelType::DELAY_STEER)
{
Eigen::VectorXd input(2);
input << msg->control.velocity, msg->control.steering_angle;
vehicle_model_ptr_->setInput(input);
Expand Down Expand Up @@ -452,10 +454,11 @@ double Simulator::getPosZFromTrajectory(const double x, const double y)
found = true;
}
}
if (found)
if (found) {
return current_trajectory_ptr_->points.at(index).pose.position.z;
else
} else {
return 0;
}
} else {
return 0.0;
}
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -23,4 +23,4 @@ int main(int argc, char ** argv)
rclcpp::shutdown();

return 0;
};
}
Original file line number Diff line number Diff line change
@@ -1,4 +1,3 @@

/*
* Copyright 2018-2019 Autoware Foundation. All rights reserved.
*
Expand All @@ -23,15 +22,15 @@ SimModelConstantAccelTwist::SimModelConstantAccelTwist(
vx_lim_(vx_lim),
wz_lim_(wz_lim),
vx_rate_(vx_rate),
wz_rate_(wz_rate){};
wz_rate_(wz_rate) {}

double SimModelConstantAccelTwist::getX() { return state_(IDX::X); };
double SimModelConstantAccelTwist::getY() { return state_(IDX::Y); };
double SimModelConstantAccelTwist::getYaw() { return state_(IDX::YAW); };
double SimModelConstantAccelTwist::getVx() { return state_(IDX::VX); };
double SimModelConstantAccelTwist::getWz() { return state_(IDX::WZ); };
double SimModelConstantAccelTwist::getSteer() { return 0.0; };
void SimModelConstantAccelTwist::update(const double & dt) { updateRungeKutta(dt, input_); }
double SimModelConstantAccelTwist::getX() {return state_(IDX::X);}
double SimModelConstantAccelTwist::getY() {return state_(IDX::Y);}
double SimModelConstantAccelTwist::getYaw() {return state_(IDX::YAW);}
double SimModelConstantAccelTwist::getVx() {return state_(IDX::VX);}
double SimModelConstantAccelTwist::getWz() {return state_(IDX::WZ);}
double SimModelConstantAccelTwist::getSteer() {return 0.0;}
void SimModelConstantAccelTwist::update(const double & dt) {updateRungeKutta(dt, input_);}
Eigen::VectorXd SimModelConstantAccelTwist::calcModel(
const Eigen::VectorXd & state, const Eigen::VectorXd & input)
{
Expand Down Expand Up @@ -62,4 +61,4 @@ Eigen::VectorXd SimModelConstantAccelTwist::calcModel(
d_state(IDX::WZ) = wz_rate;

return d_state;
};
}
Loading