ws_path="/home/vgd/code/2025_Navigation"
ros_path="/opt/ros/humble/setup.bash"
autoaim_path="/home/vgd/code/teb_ws/install"
log_path="/home/vgd/code/2025_Navigation/Logs"
cd $ws_path
colcon build

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;source;export ROS_LOG_DIR=$log_path/nav_serial; ros2 run nav_serial nav_serial; exec bash;"
sleep 1.0
echo Successfully set serial node

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;source;export ROS_LOG_DIR=$log_path/Livox_ros_driver2;ros2 launch livox_ros_driver2 msg_MID360_launch.py; exec bash;"
sleep 3.0
echo Successfully get livox lidar msgs

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/imu_complementary_filter; ros2 launch imu_complementary_filter complementary_filter.launch.py ; exec bash;"
sleep 1.0
echo Successfully launch imu filter

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/livox_pointcloud_filter; ros2 launch livox_pointcloud_filter lidar_filter_launch.py; exec bash;"
sleep 1.0
echo Successfully launch pointcloud filter

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/PointCloud_Publisher; ros2 launch point_cloud_processor point_cloud_processor_launch.py; exec bash;"
sleep 1.0
echo Successfully launch pointcloud publisher

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/linefit_ground_segmentation_ros; ros2 launch linefit_ground_segmentation_ros segmentation.launch.py; exec bash;"
sleep 1.0
echo Successfully launch point cloud segmentation

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/Fast_LIO; ros2 launch fast_lio mapping_mid360.launch.py; exec bash;"
sleep 2.0
echo Successfully launch FAST-LIO2

#gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/pointcloud_to_laserscan; ros2 launch pointcloud_to_laserscan pointcloud_to_laserscan_launch.py; exec bash;"
#sleep 1.0
#$echo Successfully launch pointcloud to laserscan

# gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/icp_localization_ros2; ros2 launch icp_localization_ros2 bringup.launch.py; exec bash;"
# sleep 3.0
# echo Successfully launch relocalization

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/pose_processing; ros2 launch pose_processing pose_processing_launch.py; exec bash;"
sleep 3.0
echo Successfully launch pose_processor

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/odom_to_baselink; ros2 launch odom_to_baselink odom_to_baselink.launch.py; exec bash;"
sleep 3.0
echo Successfully launch odom to base_link tf layer

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/lidar_localization_ros2; ros2 launch lidar_localization_ros2 lidar_localization.launch.py; exec bash;"
sleep 1.5
echo Successfully launch localization

#gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash; ros2 run odom_to_baselink odom_to_baselink; exec bash;"
#sleep 1
#echo Successfully launch odom to base_link tf layer

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash; source $autoaim_path/local_setup.bash;export ROS_LOG_DIR=$log_path/rm_navigation; ros2 launch rm_navigation bringup_launch.py; exec bash;"
sleep 3.0
echo Successfully launch navigation

gnome-terminal -- bash -c "cd $ws_path; source $ros_path; source install/setup.bash;export ROS_LOG_DIR=$log_path/pub_a_goal; ros2 run pub_a_goal pub_a_goal; exec bash;"
sleep 1.0
echo Successfully publish goal