Skip to content

Commit

Permalink
chore: source build merge main (#88)
Browse files Browse the repository at this point in the history
* chore: decrease pose_smoothing_step 5->1 (#59) (#60)

* feat: add sensorkit swtching (#57)

* feat: add sensor kit change for simulation

* chore: add ignore and pre-commit fix

* chore: aichallenge/workspace/.gitignore

Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

* fix: add simulation arg

---------

Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

* chore: disable localhost only (#86)

* feat: set same launch as source build (#82)

* feat: add tunable param

* feat: set same launch as source build

* fix: modify sensor configuration (#50)

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

* feat: add steer tire angle gain (#67)

* feat: add steering tire angle gain

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

* fix: modify steering_tire_angle_gain according to rosbag

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

---------

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

* feat: draft accel brake map (#70)

* chore: remove unused param

* feat:draft accel brake map

* fix: modify angular velocity offset (#69)

* fix: modify angular velocity offset

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

* modify

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

---------

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

* feat: add raw publisher (#71)

* fix: source build build fix control point size (#73)

* chore: decrease min point size and acc

* Update aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp

Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

* Update aichallenge/workspace/src/aichallenge_submit/simple_pure_pursuit/src/simple_pure_pursuit.cpp

---------

Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>

* feat: add pure pursuit debug (#74)

* fix: ll2 interval (#75)

* fix: modify accel map

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>

* chore: apply pre-commit

---------

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>
Co-authored-by: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: Masahiro Kubota <norikenpi@gmail.com>
Co-authored-by: knorrrr <kunotomoki61@gmail.com>

* chore: remove same launch

* fix: removed simulation arg

* chore: remove aichallenge launch

* chore: split sim time arg

* chore: how to use args

* fix: removed simulation arg (#87)

* fix: removed simulation arg

* chore: split sim time arg

* fix: removed simulation arg (#90)

* fix: removed simulation arg

* chore: split sim time arg

* feat: add topic tools

* Revert "chore: remove aichallenge launch"

This reverts commit d0405f8.

* chore: separate sim param (#94)

---------

Signed-off-by: Masahiro Kubota <norikenpi@gmail.com>
Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com>
Co-authored-by: knorrrr <99851410+knorrrr@users.noreply.github.com>
Co-authored-by: Masahiro Kubota <42679530+masahiro-kubota@users.noreply.github.com>
Co-authored-by: Masahiro Kubota <norikenpi@gmail.com>
Co-authored-by: knorrrr <kunotomoki61@gmail.com>
  • Loading branch information
6 people authored Sep 13, 2024
1 parent a57d620 commit c774316
Show file tree
Hide file tree
Showing 36 changed files with 90 additions and 378 deletions.
2 changes: 1 addition & 1 deletion Dockerfile
Original file line number Diff line number Diff line change
Expand Up @@ -11,7 +11,7 @@ RUN apt-get -y install ros-humble-rqt-graph
# PATH="$PATH:/root/.local/bin"
# PATH="/usr/local/cuda/bin:$PATH"
ENV XDG_RUNTIME_DIR=/tmp/xdg
ENV ROS_LOCALHOST_ONLY=1
ENV ROS_LOCALHOST_ONLY=0
ENV RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

FROM common AS dev
Expand Down
4 changes: 3 additions & 1 deletion aichallenge/pkill_all.sh
Original file line number Diff line number Diff line change
@@ -1,3 +1,5 @@
#!/bin/bash

pkill -9 component_conta
pkill -9 ekf_localizer
pkill -9 empty_objects_p
Expand All @@ -16,4 +18,4 @@ pkill -9 routing_adaptor
pkill -9 rviz2
pkill -9 simple_pure_pur
pkill -9 twist2accel
pkill -9 vehicle_velocit
pkill -9 vehicle_velocit
Original file line number Diff line number Diff line change
Expand Up @@ -2,4 +2,4 @@

source install/setup.bash

ros2 launch aichallenge_launch aichallenge_system.launch.xml simulation:=false
ros2 launch aichallenge_launch aichallenge_system.launch.xml simulation:=false use_sim_time:=true
5 changes: 5 additions & 0 deletions aichallenge/workspace/run_aichallenge_launch_run_vehicle.sh
Original file line number Diff line number Diff line change
@@ -0,0 +1,5 @@
#!/usr/bin/bash

source install/setup.bash

ros2 launch aichallenge_launch aichallenge_system.launch.xml simulation:=false use_sim_time:=false
2 changes: 1 addition & 1 deletion aichallenge/workspace/run_aichallenge_launch_simulation.sh
Original file line number Diff line number Diff line change
Expand Up @@ -2,4 +2,4 @@

source install/setup.bash

ros2 launch aichallenge_launch aichallenge_system.launch.xml simulation:=true
ros2 launch aichallenge_launch aichallenge_system.launch.xml simulation:=true use_sim_time:=true
Original file line number Diff line number Diff line change
Expand Up @@ -4,4 +4,4 @@ project(aichallenge_launch)
find_package(autoware_cmake REQUIRED)
autoware_package()

ament_auto_package(INSTALL_TO_SHARE launch config)
ament_auto_package(INSTALL_TO_SHARE launch)

This file was deleted.

This file was deleted.

This file was deleted.

This file was deleted.

Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@
<log message="echo launch param use_sim_time: $(var use_sim_time) launch_vehicle_interface: $(var launch_vehicle_interface) sensor_model: $(var sensor_model)"/>
<log message="echo launch param simulation: $(var simulation)"/>

<include file="$(find-pkg-share aichallenge_launch)/launch/aichallenge_submit.launch.xml" >
<include file="$(find-pkg-share aichallenge_submit_launch)/launch/aichallenge_submit.launch.xml" >
<arg name="sensor_model" value="$(var sensor_model)"/>
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>
Expand All @@ -18,11 +18,11 @@
<!-- Workaround because the simulator cannot use the service. -->
<group if="$(var simulation)">
<node pkg="aichallenge_system_launch" exec="control_mode_adapter.py" output="screen"/>
<node pkg="aichallenge_system_launch" exec="object_marker.py" output="screen"/>
</group>

<!-- RViz -->
<group>
<node pkg="rviz2" exec="rviz2" name="rviz2" output="screen" args="-d $(var rviz_config) -s $(find-pkg-share aichallenge_system_launch)/rviz/image/autoware.png"/>
<node pkg="aichallenge_system_launch" exec="object_marker.py" output="screen"/>
</group>
</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
ros__parameters:
angular_velocity_offset_x: 0.0 # [rad/s]
angular_velocity_offset_y: 0.0 # [rad/s]
angular_velocity_offset_z: 0.0 # [rad/s]
angular_velocity_offset_z: -0.004 # [rad/s]
angular_velocity_stddev_xx: 0.03 # [rad/s]
angular_velocity_stddev_yy: 0.03 # [rad/s]
angular_velocity_stddev_zz: 0.03 # [rad/s]
Original file line number Diff line number Diff line change
Expand Up @@ -7,4 +7,4 @@ default, 0.0 ,2.0 ,4.0 ,6.0 ,8.0 ,10.0
0.5, 0.175 ,0.175 ,0.175 ,0.175 ,0.175 ,0.175
0.6, 0.25 ,0.25 ,0.25 ,0.25 ,0.25 ,0.25
0.8, 0.7 ,0.7 ,0.7 ,0.7 ,0.7 ,0.7
1.0, 1.0 ,1.0 ,1.0 ,1.0 ,1.0 ,1.0
1.0, 1.0 ,1.0 ,1.0 ,1.0 ,1.0 ,1.0
Original file line number Diff line number Diff line change
Expand Up @@ -6,4 +6,4 @@
0.4, -1.1, -1.1, -1.1, -1.1, -1.1, -1.1
0.5, -1.3, -1.3, -1.3, -1.3, -1.3, -1.3
0.6, -1.5, -1.5, -1.5, -1.5, -1.5, -1.5
1.0, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5
1.0, -2.5, -2.5, -2.5, -2.5, -2.5, -2.5
Original file line number Diff line number Diff line change
Expand Up @@ -4,13 +4,15 @@
<arg name="use_sim_time" default="true"/>
<arg name="launch_vehicle_interface" default="false"/>
<arg name="sensor_model" default="racing_kart_sensor_kit"/>
<arg name="simulation" default="true"/>
<log message="echo launch param use_sim_time: $(var use_sim_time) launch_vehicle_interface: $(var launch_vehicle_interface) sensor_model: $(var sensor_model)"/>

<include file="$(find-pkg-share aichallenge_submit_launch)/launch/reference.launch.xml" >
<arg name="vehicle_model" value="racing_kart"/>
<arg name="sensor_model" value="$(var sensor_model)"/>
<arg name="map_path" value="$(find-pkg-share aichallenge_submit_launch)/map"/>
<arg name="use_sim_time" value="$(var use_sim_time)"/>
<arg name="simulation" value="$(var simulation)"/>
<arg name="launch_vehicle_interface" value="$(var launch_vehicle_interface)"/>
</include>

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -2,20 +2,19 @@
<launch>
<!-- Essential parameters -->
<arg name="vehicle_id" default="$(env VEHICLE_ID default)" description="vehicle specific ID"/>
<arg name="use_sim_time" default="true"/>
<arg name="map_path" default="/aichallenge/mapfile"/>
<arg name="use_sim_time" default="false"/>
<arg name="map_path" default="$(find-pkg-share aichallenge_submit_launch)/map"/>
<arg name="vehicle_model" default="racing_kart" description="vehicle model name"/>
<arg name="sensor_model" description="sensor model name"/>

<arg name="sensor_model" default="racing_kart_sensor_kit" description="sensor model name"/>
<arg name="simulation" description="used for sensor kit param"/>
<!-- Optional parameters -->
<!-- Map -->
<arg name="lanelet2_map_file" default="lanelet2_map.osm" description="lanelet2 map file name"/>
<arg name="pointcloud_map_file" default="pointcloud_map.pcd" description="pointcloud map file name"/>
<!-- Control -->

<!-- Vehicle -->
<arg name="launch_vehicle_interface" default="false"/>
<!-- System -->
<arg name="system_run_mode" default="online" description="run mode in system"/>
<log message="echo launch param use_sim_time: $(var use_sim_time) launch_vehicle_interface: $(var launch_vehicle_interface) sensor_model: $(var sensor_model)"/>

<!-- Global parameters -->
<group scoped="false">
Expand All @@ -29,7 +28,7 @@
<arg name="model_file" default="$(find-pkg-share tier4_vehicle_launch)/urdf/vehicle.xacro" description="path to the file of model settings (*.xacro)"/>
<arg name="config_dir" default="$(find-pkg-share racing_kart_sensor_kit_description)/config"/>
<node name="robot_state_publisher" pkg="robot_state_publisher" exec="robot_state_publisher">
<param name="robot_description" value="$(command 'xacro $(var model_file) vehicle_model:=$(var vehicle_model) sensor_model:=$(var sensor_model) config_dir:=$(var config_dir)' 'warn')"/>
<param name="robot_description" value="$(command 'xacro $(var model_file) vehicle_model:=$(var vehicle_model) sensor_model:=$(var sensor_model) config_dir:=$(var config_dir) simulation:=$(var simulation)' 'warn')"/>
</node>
</group>

Expand All @@ -40,19 +39,22 @@
<include file="$(find-pkg-share vehicle_velocity_converter)/launch/vehicle_velocity_converter.launch.xml">
<arg name="input_vehicle_velocity_topic" value="/vehicle/status/velocity_status"/>
<arg name="output_twist_with_covariance" value="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
<arg name="config_file" value="$(find-pkg-share aichallenge_submit_launch)/config/vehicle_velocity_converter.param.yaml"/>
</include>
<group>
<push-ros-namespace namespace="imu"/>
<arg name="imu_corrector_param_file" default="$(find-pkg-share aichallenge_submit_launch)/config/imu_corrector.param.yaml"/>
<include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
<arg name="input_topic" value="imu_raw"/>
<arg name="output_topic" value="imu_data"/>
<arg name="param_file" value="$(var imu_corrector_param_file)"/>
</include>
<include file="$(find-pkg-share imu_corrector)/launch/imu_corrector.launch.xml">
<arg name="input_topic" value="imu_raw"/>
<arg name="output_topic" value="imu_data"/>
<arg name="param_file" value="$(var imu_corrector_param_file)"/>
</include>
</group>
</group>

<!-- Localization -->
<!-- Localization -->
<let name="pose_additional_delay_var" value="0.0" if="$(var simulation)"/>
<let name="pose_additional_delay_var" value="0.5" unless="$(var simulation)"/>
<group>
<push-ros-namespace namespace="localization"/>
<include file="$(find-pkg-share gyro_odometer)/launch/gyro_odometer.launch.xml">
Expand All @@ -68,6 +70,7 @@
<arg name="enable_yaw_bias_estimation" value="false"/>
<arg name="tf_rate" value="50.0"/>
<arg name="twist_smoothing_steps" value="1"/>
<arg name="pose_smoothing_steps" value="1"/>
<arg name="input_initial_pose_name" value="/localization/initial_pose3d"/>
<arg name="input_pose_with_cov_name" value="/localization/imu_gnss_poser/pose_with_covariance"/>
<arg name="input_twist_with_cov_name" value="/localization/twist_estimator/twist_with_covariance"/>
Expand All @@ -80,6 +83,8 @@
<arg name="output_twist_with_covariance_name" value="twist_with_covariance"/>
<arg name="proc_stddev_vx_c" value="10.0"/>
<arg name="proc_stddev_wz_c" value="5.0"/>
<arg name="pose_additional_delay" value="$(var pose_additional_delay_var)"/>
<arg name="extend_state_step" value="100"/>
</include>

<!-- twist2accel -->
Expand Down Expand Up @@ -203,12 +208,17 @@
</group>
</group>

<!-- Pure Pursuit -->
<let name="steering_tire_angle_gain_var" value="1.0" if="$(var simulation)"/>
<let name="steering_tire_angle_gain_var" value="1.639" unless="$(var simulation)"/>

<node pkg="simple_pure_pursuit" exec="simple_pure_pursuit" name="simple_pure_pursuit_node" output="screen">
<param name="use_external_target_vel" value="false"/>
<param name="external_target_vel" value="8.0"/>
<param name="lookahead_gain" value="0.5"/>
<param name="lookahead_min_distance" value="3.0"/>
<param name="speed_proportional_gain" value="1.0"/>
<param name="steering_tire_angle_gain" value="$(var steering_tire_angle_gain_var)"/>

<remap from="input/kinematics" to="/localization/kinematic_state"/>
<remap from="input/trajectory" to="/planning/scenario_planning/trajectory"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -17,4 +17,4 @@ ament_auto_add_executable(goal_pose_setter_node
ament_auto_package(
INSTALL_TO_SHARE
config
)
)
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
ros__parameters:
angular_velocity_offset_x: 0.0 # [rad/s]
angular_velocity_offset_y: 0.0 # [rad/s]
angular_velocity_offset_z: -0.004 # [rad/s]
angular_velocity_offset_z: 0.0 # [rad/s]
angular_velocity_stddev_xx: 0.03 # [rad/s]
angular_velocity_stddev_yy: 0.03 # [rad/s]
angular_velocity_stddev_zz: 0.03 # [rad/s]
Original file line number Diff line number Diff line change
Expand Up @@ -16,4 +16,4 @@ ament_auto_add_executable(imu_gnss_poser_node

ament_auto_package(
INSTALL_TO_SHARE
)
)
Original file line number Diff line number Diff line change
Expand Up @@ -14,4 +14,4 @@ ament_auto_add_executable(path_to_trajectory_node
src/path_to_trajectory.cpp
)

ament_auto_package()
ament_auto_package()
Original file line number Diff line number Diff line change
Expand Up @@ -34,4 +34,4 @@ class PathToTrajectory : public rclcpp::Node {
rclcpp::Publisher<Trajectory>::SharedPtr pub_;
};

#endif // PATH_TO_TRAJECTORY__PATH_TO_TRAJECTORY_HPP_
#endif // PATH_TO_TRAJECTORY__PATH_TO_TRAJECTORY_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -12,4 +12,4 @@ ament_auto_package(INSTALL_TO_SHARE
mesh
urdf
config
)
)
Original file line number Diff line number Diff line change
Expand Up @@ -9,4 +9,4 @@
left_overhang: 0.09 # between rear left wheel center and vehicle left (the half of the rear wheel width)
right_overhang: 0.09 # between rear right wheel center and vehicle right (the half of the rear wheel width)
vehicle_height: 2.2 # vehicle height + sensor_kit height
max_steer_angle: 0.64 # [rad]
max_steer_angle: 0.64 # [rad]
Original file line number Diff line number Diff line change
Expand Up @@ -1254,4 +1254,4 @@
<scene>
<instance_visual_scene url="#699211dd-c421-4a6e-8573-f8cc1c1bcaad"></instance_visual_scene>
</scene>
</COLLADA>
</COLLADA>
Original file line number Diff line number Diff line change
Expand Up @@ -13,4 +13,4 @@
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
</package>
Original file line number Diff line number Diff line change
Expand Up @@ -14,4 +14,4 @@
</visual>
</link>

</robot>
</robot>
Original file line number Diff line number Diff line change
Expand Up @@ -8,4 +8,4 @@ ament_auto_find_build_dependencies()
ament_auto_package(INSTALL_TO_SHARE
urdf
config
)
)
Original file line number Diff line number Diff line change
@@ -0,0 +1,15 @@
sensor_kit_base_link:
gnss_link:
x: 0.0
y: 0.0
z: 0.0
roll: 0.0
pitch: 0.0
yaw: 0.0
imu_link:
x: 0.0
y: 0.0
z: 0.0
roll: 0.0
pitch: 0.0
yaw: 0.0
Original file line number Diff line number Diff line change
Expand Up @@ -19,4 +19,4 @@ sensor_kit_base_link:
z: 0.0
roll: 0.0
pitch: 0.0
yaw: -1.57
yaw: -1.5707963268
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,7 @@

<xacro:arg name="gpu" default="false"/>
<xacro:arg name="config_dir" default="$(find racing_kart_sensor_kit_description)/config"/>
<xacro:arg name="simulation" default="false"/>

<xacro:property name="sensor_kit_base_link" default="sensor_kit_base_link"/>

Expand All @@ -18,8 +19,11 @@
</link>

<!-- sensor -->
<xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/sensor_kit_calibration.yaml')}"/>

<xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/sensor_kit_calibration.yaml')}"/>
<!-- over write calibration file for simulation -->
<xacro:if value="$(arg simulation)">
<xacro:property name="calibration" value="${xacro.load_yaml('$(arg config_dir)/awsim_sensor_kit_calibration.yaml')}"/>
</xacro:if>
<!-- gnss -->
<xacro:imu_macro
name="gnss"
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,4 +13,4 @@ ament_auto_add_executable(simple_pure_pursuit
src/simple_pure_pursuit.cpp
)

ament_auto_package()
ament_auto_package()
Original file line number Diff line number Diff line change
Expand Up @@ -60,4 +60,4 @@ class SimplePurePursuit : public rclcpp::Node {

} // namespace simple_pure_pursuit

#endif // SIMPLE_PURE_PURSUIT_HPP_
#endif // SIMPLE_PURE_PURSUIT_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -17,4 +17,4 @@
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
</package>
Loading
Sorry, something went wrong. Reload?
Sorry, we cannot display this file.
Sorry, this file is invalid so it cannot be displayed.
Loading

0 comments on commit c774316

Please sign in to comment.