diff --git a/README.md b/README.md index 3b9a3b2..c040200 100644 --- a/README.md +++ b/README.md @@ -77,6 +77,3 @@ ros2 run teleop_twist_keyboard teleop_twist_keyboard Then, press `i` to move forward, `u` and `o` to move forward and turn, `,` to move backwards, `m` and `.` to move backwards and turn, and `k` to stop in the terminal window running the teleop node. -## About Us - -S7our Squad team at the University of Ibn Tofail, we work on autonomous systems (cars), led by professor BOUKIR Khawla. Current members composed of OKHADIR Hamza, GHIATI Mustapha, KHATIB Mouad and EL-LOUH Youssef. diff --git a/error b/error new file mode 100644 index 0000000..6aa9885 --- /dev/null +++ b/error @@ -0,0 +1,737 @@ +riftpilot@driftpilot:~$ rm -rf build install log +driftpilot@driftpilot:~$ cd RC-CAR-ROS2/ +driftpilot@driftpilot:~/RC-CAR-ROS2$ rm -rf build install log +driftpilot@driftpilot:~/RC-CAR-ROS2$ colcon build +Starting >>> ydlidar_ros2_driver +[1.015s] WARNING:colcon.colcon_core.prefix_path.colcon:The path '/home/driftpilot/install' in the environment variable COLCON_PREFIX_PATH doesn't exist +[1.015s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/RC-CAR-ROS2/install/wall_follow' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.015s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/RC-CAR-ROS2/install/simulator' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.015s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/RC-CAR-ROS2/install/safety_node' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.015s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/RC-CAR-ROS2/install/oled_eyes' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.015s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/RC-CAR-ROS2/install/manual_ctrl' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.015s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/RC-CAR-ROS2/install/llm_ctrl' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.015s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/RC-CAR-ROS2/install/ld_ctrl' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/RC-CAR-ROS2/install/gap_follow' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/install/wall_follow' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/install/simulator' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/install/safety_node' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/install/oled_eyes' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/install/manual_ctrl' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/install/llm_ctrl' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/install/ld_ctrl' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.ament:The path '/home/driftpilot/install/gap_follow' in the environment variable AMENT_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/driftpilot/RC-CAR-ROS2/install/wall_follow' in the environment variable CMAKE_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/driftpilot/RC-CAR-ROS2/install/safety_node' in the environment variable CMAKE_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/driftpilot/RC-CAR-ROS2/install/gap_follow' in the environment variable CMAKE_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/driftpilot/install/ydlidar_sdk' in the environment variable CMAKE_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/driftpilot/install/wall_follow' in the environment variable CMAKE_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/driftpilot/install/safety_node' in the environment variable CMAKE_PREFIX_PATH doesn't exist +[1.016s] WARNING:colcon.colcon_ros.prefix_path.catkin:The path '/home/driftpilot/install/gap_follow' in the environment variable CMAKE_PREFIX_PATH doesn't exist +Starting >>> gap_follow +Starting >>> llm_ctrl +Starting >>> manual_ctrl +Finished <<< llm_ctrl [4.62s] +Starting >>> oled_eyes +Finished <<< manual_ctrl [4.64s] +Starting >>> safety_node +Finished <<< oled_eyes [4.91s] +Starting >>> simulator +Finished <<< simulator [5.47s] +Starting >>> wall_follow +--- stderr: ydlidar_ros2_driver +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp: In function ‘int main(int, char**)’: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:44:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [5])’ + 44 | node->declare_parameter("port"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~ +In file included from /opt/ros/jazzy/include/rclcpp/rclcpp/executors/single_threaded_executor.hpp:28, + from /opt/ros/jazzy/include/rclcpp/rclcpp/executors.hpp:22, + from /opt/ros/jazzy/include/rclcpp/rclcpp/rclcpp.hpp:172, + from /home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:23: +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:44:26: note: candidate expects 4 arguments, 1 provided + 44 | node->declare_parameter("port"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:44:26: note: couldn’t deduce template parameter ‘ParameterT’ + 44 | node->declare_parameter("port"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:51:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [13])’ + 51 | node->declare_parameter("ignore_array"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:51:26: note: candidate expects 4 arguments, 1 provided + 51 | node->declare_parameter("ignore_array"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:51:26: note: couldn’t deduce template parameter ‘ParameterT’ + 51 | node->declare_parameter("ignore_array"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:56:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [9])’ + 56 | node->declare_parameter("frame_id"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:56:26: note: candidate expects 4 arguments, 1 provided + 56 | node->declare_parameter("frame_id"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:56:26: note: couldn’t deduce template parameter ‘ParameterT’ + 56 | node->declare_parameter("frame_id"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:62:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [9])’ + 62 | node->declare_parameter("baudrate"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:62:26: note: candidate expects 4 arguments, 1 provided + 62 | node->declare_parameter("baudrate"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:62:26: note: couldn’t deduce template parameter ‘ParameterT’ + 62 | node->declare_parameter("baudrate"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:67:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [11])’ + 67 | node->declare_parameter("lidar_type"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:67:26: note: candidate expects 4 arguments, 1 provided + 67 | node->declare_parameter("lidar_type"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:67:26: note: couldn’t deduce template parameter ‘ParameterT’ + 67 | node->declare_parameter("lidar_type"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:72:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [12])’ + 72 | node->declare_parameter("device_type"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:72:26: note: candidate expects 4 arguments, 1 provided + 72 | node->declare_parameter("device_type"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:72:26: note: couldn’t deduce template parameter ‘ParameterT’ + 72 | node->declare_parameter("device_type"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:77:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [12])’ + 77 | node->declare_parameter("sample_rate"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:77:26: note: candidate expects 4 arguments, 1 provided + 77 | node->declare_parameter("sample_rate"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:77:26: note: couldn’t deduce template parameter ‘ParameterT’ + 77 | node->declare_parameter("sample_rate"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:82:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [21])’ + 82 | node->declare_parameter("abnormal_check_count"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:82:26: note: candidate expects 4 arguments, 1 provided + 82 | node->declare_parameter("abnormal_check_count"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:82:26: note: couldn’t deduce template parameter ‘ParameterT’ + 82 | node->declare_parameter("abnormal_check_count"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:88:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [14])’ + 88 | node->declare_parameter("intensity_bit"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:88:26: note: candidate expects 4 arguments, 1 provided + 88 | node->declare_parameter("intensity_bit"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:88:26: note: couldn’t deduce template parameter ‘ParameterT’ + 88 | node->declare_parameter("intensity_bit"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:95:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [17])’ + 95 | node->declare_parameter("fixed_resolution"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:95:26: note: candidate expects 4 arguments, 1 provided + 95 | node->declare_parameter("fixed_resolution"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:95:26: note: couldn’t deduce template parameter ‘ParameterT’ + 95 | node->declare_parameter("fixed_resolution"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:100:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [10])’ + 100 | node->declare_parameter("reversion"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:100:26: note: candidate expects 4 arguments, 1 provided + 100 | node->declare_parameter("reversion"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:100:26: note: couldn’t deduce template parameter ‘ParameterT’ + 100 | node->declare_parameter("reversion"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:105:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [9])’ + 105 | node->declare_parameter("inverted"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:105:26: note: candidate expects 4 arguments, 1 provided + 105 | node->declare_parameter("inverted"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:105:26: note: couldn’t deduce template parameter ‘ParameterT’ + 105 | node->declare_parameter("inverted"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:109:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [15])’ + 109 | node->declare_parameter("auto_reconnect"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:109:26: note: candidate expects 4 arguments, 1 provided + 109 | node->declare_parameter("auto_reconnect"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:109:26: note: couldn’t deduce template parameter ‘ParameterT’ + 109 | node->declare_parameter("auto_reconnect"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:114:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [16])’ + 114 | node->declare_parameter("isSingleChannel"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:114:26: note: candidate expects 4 arguments, 1 provided + 114 | node->declare_parameter("isSingleChannel"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:114:26: note: couldn’t deduce template parameter ‘ParameterT’ + 114 | node->declare_parameter("isSingleChannel"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:119:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [10])’ + 119 | node->declare_parameter("intensity"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:119:26: note: candidate expects 4 arguments, 1 provided + 119 | node->declare_parameter("intensity"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:119:26: note: couldn’t deduce template parameter ‘ParameterT’ + 119 | node->declare_parameter("intensity"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:124:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [18])’ + 124 | node->declare_parameter("support_motor_dtr"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:124:26: note: candidate expects 4 arguments, 1 provided + 124 | node->declare_parameter("support_motor_dtr"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:124:26: note: couldn’t deduce template parameter ‘ParameterT’ + 124 | node->declare_parameter("support_motor_dtr"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:129:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [6])’ + 129 | node->declare_parameter("debug"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:129:26: note: candidate expects 4 arguments, 1 provided + 129 | node->declare_parameter("debug"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:129:26: note: couldn’t deduce template parameter ‘ParameterT’ + 129 | node->declare_parameter("debug"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:136:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [10])’ + 136 | node->declare_parameter("angle_max"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:136:26: note: candidate expects 4 arguments, 1 provided + 136 | node->declare_parameter("angle_max"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:136:26: note: couldn’t deduce template parameter ‘ParameterT’ + 136 | node->declare_parameter("angle_max"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:140:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [10])’ + 140 | node->declare_parameter("angle_min"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:140:26: note: candidate expects 4 arguments, 1 provided + 140 | node->declare_parameter("angle_min"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:140:26: note: couldn’t deduce template parameter ‘ParameterT’ + 140 | node->declare_parameter("angle_min"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:145:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [10])’ + 145 | node->declare_parameter("range_max"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:145:26: note: candidate expects 4 arguments, 1 provided + 145 | node->declare_parameter("range_max"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:145:26: note: couldn’t deduce template parameter ‘ParameterT’ + 145 | node->declare_parameter("range_max"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:149:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [10])’ + 149 | node->declare_parameter("range_min"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:149:26: note: candidate expects 4 arguments, 1 provided + 149 | node->declare_parameter("range_min"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:149:26: note: couldn’t deduce template parameter ‘ParameterT’ + 149 | node->declare_parameter("range_min"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:154:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [10])’ + 154 | node->declare_parameter("frequency"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:154:26: note: candidate expects 4 arguments, 1 provided + 154 | node->declare_parameter("frequency"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:154:26: note: couldn’t deduce template parameter ‘ParameterT’ + 154 | node->declare_parameter("frequency"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:159:26: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [21])’ + 159 | node->declare_parameter("invalid_range_is_inf"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:159:26: note: candidate expects 4 arguments, 1 provided + 159 | node->declare_parameter("invalid_range_is_inf"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:159:26: note: couldn’t deduce template parameter ‘ParameterT’ + 159 | node->declare_parameter("invalid_range_is_inf"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:168:28: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [8])’ + 168 | node->declare_parameter("m1_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:168:28: note: candidate expects 4 arguments, 1 provided + 168 | node->declare_parameter("m1_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:168:28: note: couldn’t deduce template parameter ‘ParameterT’ + 168 | node->declare_parameter("m1_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:172:28: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [8])’ + 172 | node->declare_parameter("m2_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:172:28: note: candidate expects 4 arguments, 1 provided + 172 | node->declare_parameter("m2_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:172:28: note: couldn’t deduce template parameter ‘ParameterT’ + 172 | node->declare_parameter("m2_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:176:28: error: no matching function for call to ‘rclcpp::Node::declare_parameter(const char [8])’ + 176 | node->declare_parameter("m3_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const ParameterT&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 500 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:500:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:176:28: note: candidate expects 4 arguments, 1 provided + 176 | node->declare_parameter("m3_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: candidate: ‘template auto rclcpp::Node::declare_parameter(const std::string&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 513 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:513:3: note: template argument deduction/substitution failed: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:176:28: note: couldn’t deduce template parameter ‘ParameterT’ + 176 | node->declare_parameter("m3_mode"); + | ~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, const rclcpp::ParameterValue&, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 445 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:445:3: note: candidate expects 4 arguments, 1 provided +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate: ‘const rclcpp::ParameterValue& rclcpp::Node::declare_parameter(const std::string&, rclcpp::ParameterType, const rcl_interfaces::msg::ParameterDescriptor&, bool)’ + 470 | declare_parameter( + | ^~~~~~~~~~~~~~~~~ +/opt/ros/jazzy/include/rclcpp/rclcpp/node.hpp:470:3: note: candidate expects 4 arguments, 1 provided +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp: In lambda function: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:190:54: warning: unused parameter ‘request_header’ [-Wunused-parameter] + 190 | [&laser](const std::shared_ptr request_header, + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~ +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:191:56: warning: unused parameter ‘req’ [-Wunused-parameter] + 191 | const std::shared_ptr req, + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~ +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:192:51: warning: unused parameter ‘response’ [-Wunused-parameter] + 192 | std::shared_ptr response) -> bool + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~ +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp: In lambda function: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:200:54: warning: unused parameter ‘request_header’ [-Wunused-parameter] + 200 | [&laser](const std::shared_ptr request_header, + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~ +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:201:56: warning: unused parameter ‘req’ [-Wunused-parameter] + 201 | const std::shared_ptr req, + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~ +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:202:51: warning: unused parameter ‘response’ [-Wunused-parameter] + 202 | std::shared_ptr response) -> bool + | ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~ +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp: In function ‘int main(int, char**)’: +/home/driftpilot/RC-CAR-ROS2/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp:235:21: warning: unused variable ‘p’ [-Wunused-variable] + 235 | const auto& p = scan.points.at(i); + | ^ +gmake[2]: *** [CMakeFiles/ydlidar_ros2_driver_node.dir/build.make:76: CMakeFiles/ydlidar_ros2_driver_node.dir/src/ydlidar_ros2_driver_node.cpp.o] Error 1 +gmake[1]: *** [CMakeFiles/Makefile2:139: CMakeFiles/ydlidar_ros2_driver_node.dir/all] Error 2 +gmake[1]: *** Waiting for unfinished jobs.... +gmake: *** [Makefile:146: all] Error 2 +--- +Failed <<< ydlidar_ros2_driver [32.1s, exited with code 2] +Aborted <<< gap_follow [32.7s] +Aborted <<< safety_node [37.3s] +Aborted <<< wall_follow [27.3s] + +Summary: 4 packages finished [42.7s] + 1 package failed: ydlidar_ros2_driver + 3 packages aborted: gap_follow safety_node wall_follow + 1 package had stderr output: ydlidar_ros2_driver + 1 package not processed +driftpilot@driftpilot:~/RC-CAR-ROS2$ + diff --git a/img/IntertiaPID.png b/img/IntertiaPID.png deleted file mode 100644 index 27fd924..0000000 Binary files a/img/IntertiaPID.png and /dev/null differ diff --git a/img/wall_following_lab_figure_1.png b/img/wall_following_lab_figure_1.png deleted file mode 100644 index 386775c..0000000 Binary files a/img/wall_following_lab_figure_1.png and /dev/null differ diff --git a/img/wall_following_lab_figure_2.png b/img/wall_following_lab_figure_2.png deleted file mode 100644 index d3a6379..0000000 Binary files a/img/wall_following_lab_figure_2.png and /dev/null differ diff --git a/master.zip b/master.zip new file mode 100644 index 0000000..66e50f2 Binary files /dev/null and b/master.zip differ diff --git a/src/f1tenth_gym b/src/f1tenth_gym new file mode 160000 index 0000000..4fdb9c7 --- /dev/null +++ b/src/f1tenth_gym @@ -0,0 +1 @@ +Subproject commit 4fdb9c7e6fb7c701290f4dc18377d07c1681724f diff --git a/src/gap_follow/CMakeLists.txt b/src/gap_follow/CMakeLists.txt index 7f4174d..0836cda 100644 --- a/src/gap_follow/CMakeLists.txt +++ b/src/gap_follow/CMakeLists.txt @@ -3,7 +3,7 @@ project(gap_follow) # Default to C++14 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") @@ -30,14 +30,14 @@ endforeach() include_directories(include) # Create Cpp executable -add_executable(reactive_node src/reactive_node.cpp) -ament_target_dependencies(reactive_node +add_executable(reactive_node_cpp src/reactive_node.cpp) +ament_target_dependencies(reactive_node_cpp rclcpp geometry_msgs ackermann_msgs nav_msgs sensor_msgs std_msgs ) # Install Cpp executables install(TARGETS - reactive_node + reactive_node_cpp DESTINATION lib/${PROJECT_NAME}) # Install Python modules diff --git a/src/gap_follow/gap_follow/__init__.py b/src/gap_follow/gap_follow/__init__.py index e69de29..8c16fe3 100644 --- a/src/gap_follow/gap_follow/__init__.py +++ b/src/gap_follow/gap_follow/__init__.py @@ -0,0 +1 @@ +self.prev_steering = 0.0 diff --git a/src/gap_follow/gap_follow/gap_follow_node.py b/src/gap_follow/gap_follow/gap_follow_node.py new file mode 100644 index 0000000..cad8a94 --- /dev/null +++ b/src/gap_follow/gap_follow/gap_follow_node.py @@ -0,0 +1,146 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import Twist +import math +import numpy as np + +class GapFollowNode(Node): + def __init__(self): + super().__init__('gap_follow') + + # Subscribe to LiDAR scan + self.scan_subscription = self.create_subscription( + LaserScan, + '/scan', + self.scan_callback, + 10 + ) + + # Publish velocity commands + self.velocity_publisher = self.create_publisher( + Twist, + '/cmd_vel', + 10 + ) + + # Parameters + self.declare_parameter('linear_velocity', 0.5) + self.declare_parameter('gap_threshold', 0.5) # meters + self.declare_parameter('min_gap_width', 5) # degrees + self.declare_parameter('max_angular_velocity', 1.5) + + self.linear_velocity = self.get_parameter('linear_velocity').value + self.gap_threshold = self.get_parameter('gap_threshold').value + self.min_gap_width = self.get_parameter('min_gap_width').value + self.max_angular_velocity = self.get_parameter('max_angular_velocity').value + + self.get_logger().info('Gap Follow Node started') + self.get_logger().info(f'Target gap threshold: {self.gap_threshold}m') + + def scan_callback(self, msg: LaserScan): + """ + Process LiDAR scan and find largest gap. + """ + try: + # Convert ranges to numpy for easier processing + ranges = np.array(msg.ranges) + + # Replace infinities with max range + ranges = np.nan_to_num(ranges, nan=msg.range_max, + posinf=msg.range_max, + neginf=msg.range_max) + + # Find gaps (free space where range > threshold) + gaps = self.find_gaps(ranges, msg) + + if gaps: + # Get largest gap + largest_gap = max(gaps, key=lambda x: x[1] - x[0]) + gap_center = (largest_gap[0] + largest_gap[1]) / 2 + + # Calculate angle to gap center + gap_angle = (msg.angle_min + + gap_center * msg.angle_increment) + + # Generate movement command toward gap + cmd = Twist() + cmd.linear.x = self.linear_velocity + cmd.angular.z = self.saturate_angular_velocity( + gap_angle * 0.5, + self.max_angular_velocity + ) + + self.velocity_publisher.publish(cmd) + + self.get_logger().debug( + f'Gap found at {math.degrees(gap_angle):.2f}°' + ) + else: + # No gap found - stop + self.get_logger().warn('No gap found - stopping') + cmd = Twist() + self.velocity_publisher.publish(cmd) + + except Exception as e: + self.get_logger().error(f'Error in scan callback: {str(e)}') + + def find_gaps(self, ranges, msg): + """ + Find all gaps in range data. + A gap is a continuous region where range > gap_threshold. + Returns list of (start_index, end_index) tuples. + """ + gaps = [] + gap_start = None + + for i, r in enumerate(ranges): + if r > self.gap_threshold: # Free space + if gap_start is None: + gap_start = i + else: # Obstacle + if gap_start is not None: + # Check gap width + gap_width = i - gap_start + gap_width_degrees = gap_width * math.degrees(msg.angle_increment) + + if gap_width_degrees >= self.min_gap_width: + gaps.append((gap_start, i)) + gap_start = None + + # Handle gap that extends to end of scan + if gap_start is not None: + gap_width = len(ranges) - gap_start + gap_width_degrees = gap_width * math.degrees(msg.angle_increment) + + if gap_width_degrees >= self.min_gap_width: + gaps.append((gap_start, len(ranges))) + + return gaps + + def saturate_angular_velocity(self, angular_vel, max_vel): + """Limit angular velocity to maximum value.""" + if angular_vel > max_vel: + return max_vel + elif angular_vel < -max_vel: + return -max_vel + return angular_vel + + +def main(args=None): + rclpy.init(args=args) + gap_follow_node = GapFollowNode() + + try: + rclpy.spin(gap_follow_node) + except KeyboardInterrupt: + pass + finally: + gap_follow_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/gap_follow/package.xml b/src/gap_follow/package.xml index f95c49c..5eb0c3e 100644 --- a/src/gap_follow/package.xml +++ b/src/gap_follow/package.xml @@ -16,6 +16,7 @@ sensor_msgs ackermann_msgs geometry_msgs + ydlidar_ros2_driver ament_lint_auto ament_lint_common diff --git a/src/gap_follow/scripts/reactive_node.py b/src/gap_follow/scripts/reactive_node.py index 1a3a7b9..4183aea 100644 --- a/src/gap_follow/scripts/reactive_node.py +++ b/src/gap_follow/scripts/reactive_node.py @@ -1,72 +1,154 @@ #!/usr/bin/env python3 + import rclpy from rclpy.node import Node - import numpy as np + from sensor_msgs.msg import LaserScan -from ackermann_msgs.msg import AckermannDriveStamped, AckermannDrive +from ackermann_msgs.msg import AckermannDriveStamped + class ReactiveFollowGap(Node): - """ - Implement Wall Following on the car - This is just a template, you are free to implement your own node! - """ + def __init__(self): - super().__init__('reactive_node') - # Topics & Subs, Pubs - lidarscan_topic = '/scan' - drive_topic = '/drive' + super().__init__('reactive_follow_gap') + + # Topics + self.scan_topic = '/scan' + self.drive_topic = '/drive' + + # ===== CAR + LIDAR TUNING (FOR YOUR SETUP) ===== + self.max_range = 4.0 # meters (X2 reliable range) + self.bubble_radius = 0.45 # meters (1/10 car safety) + self.max_steering_angle = 0.34 # rad (~19.5 deg) + self.steering_gain = 0.9 + self.front_fov_deg = 70 # +- degrees + self.prev_steering = 0.0 - # TODO: Subscribe to LIDAR - # TODO: Publish to drive + # ROS interfaces + self.scan_sub = self.create_subscription( + LaserScan, + self.scan_topic, + self.lidar_callback, + 10 + ) + self.drive_pub = self.create_publisher( + AckermannDriveStamped, + self.drive_topic, + 10 + ) + + self.get_logger().info("Follow-the-Gap initialized (YDLIDAR X2, 1/10 RC)") + + # ------------------------------------------------ def preprocess_lidar(self, ranges): - """ Preprocess the LiDAR scan array. Expert implementation includes: - 1.Setting each value to the mean over some window - 2.Rejecting high values (eg. > 3m) - """ - proc_ranges = ranges - return proc_ranges - - def find_max_gap(self, free_space_ranges): - """ Return the start index & end index of the max gap in free_space_ranges - """ - return None - - def find_best_point(self, start_i, end_i, ranges): - """Start_i & end_i are start and end indicies of max-gap range, respectively - Return index of best point in ranges - Naive: Choose the furthest point within ranges and go there - """ - return None + ranges = np.array(ranges) + ranges[np.isnan(ranges)] = 0.0 + ranges[np.isinf(ranges)] = self.max_range + ranges = np.clip(ranges, 0.0, self.max_range) + return ranges + + # ------------------------------------------------ + def find_max_gap(self, ranges): + gaps = [] + start = None + + for i, r in enumerate(ranges): + if r > 0 and start is None: + start = i + elif r == 0 and start is not None: + gaps.append((start, i - 1)) + start = None + + if start is not None: + gaps.append((start, len(ranges) - 1)) + + if not gaps: + return None, None + + return max(gaps, key=lambda g: g[1] - g[0]) + # ------------------------------------------------ + def find_best_point(self, start, end, ranges): + segment = ranges[start:end + 1] + return start + np.argmax(segment) + + # ------------------------------------------------ def lidar_callback(self, data): - """ Process each LiDAR scan as per the Follow Gap algorithm & publish an AckermannDriveStamped Message - """ - ranges = data.ranges - proc_ranges = self.preprocess_lidar(ranges) - - # TODO: - #Find closest point to LiDAR + ranges = self.preprocess_lidar(data.ranges) + + # ===== FRONT-ONLY FOV ===== + front_angle = np.deg2rad(self.front_fov_deg) + + start_i = int((-front_angle - data.angle_min) / data.angle_increment) + end_i = int(( front_angle - data.angle_min) / data.angle_increment) + + start_i = max(start_i, 0) + end_i = min(end_i, len(ranges) - 1) + + ranges = ranges[start_i:end_i] + + # ===== FIND CLOSEST OBSTACLE ===== + closest_idx = np.argmin(ranges) + closest_dist = ranges[closest_idx] + + # ===== DISTANCE-AWARE BUBBLE ===== + if closest_dist > 0.0: + bubble_angle = int( + self.bubble_radius / + (closest_dist * data.angle_increment) + ) + bubble_start = max(0, closest_idx - bubble_angle) + bubble_end = min(len(ranges) - 1, closest_idx + bubble_angle) + ranges[bubble_start:bubble_end] = 0.0 - #Eliminate all points inside 'bubble' (set them to zero) + # ===== FIND GAP ===== + gap_start, gap_end = self.find_max_gap(ranges) + if gap_start is None: + return - #Find max length gap + best_idx = self.find_best_point(gap_start, gap_end, ranges) - #Find the best point in the gap + # ===== STEERING ===== + angle = ( + data.angle_min + + (best_idx + start_i) * data.angle_increment + ) * self.steering_gain - #Publish Drive message + angle = np.clip(angle, + -self.max_steering_angle, + self.max_steering_angle) + + # Steering smoothing (ESC & traction safety) + angle = 0.7 * self.prev_steering + 0.3 * angle + self.prev_steering = angle + + # ===== SPEED CONTROL (CRITICAL FOR 4300KV) ===== + min_dist = np.min(ranges) + + if min_dist < 0.8: + speed = 0.7 + elif min_dist < 1.2: + speed = 1.1 + else: + speed = 1.6 + + # ===== PUBLISH ===== + drive_msg = AckermannDriveStamped() + drive_msg.drive.steering_angle = angle + drive_msg.drive.speed = speed + + self.drive_pub.publish(drive_msg) def main(args=None): rclpy.init(args=args) - print("WallFollow Initialized") - reactive_node = ReactiveFollowGap() - rclpy.spin(reactive_node) - - reactive_node.destroy_node() + node = ReactiveFollowGap() + rclpy.spin(node) + node.destroy_node() rclpy.shutdown() if __name__ == '__main__': - main() \ No newline at end of file + main() diff --git a/src/gap_follow/setup.py b/src/gap_follow/setup.py index ef6acfd..18b59fd 100644 --- a/src/gap_follow/setup.py +++ b/src/gap_follow/setup.py @@ -4,7 +4,7 @@ setup( name=package_name, - version='0.0.0', + version='0.0.1', packages=[package_name], data_files=[ ('share/ament_index/resource_index/packages', @@ -20,7 +20,8 @@ tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'reactive_node = gap_follow.reactive_node:main', + 'reactive_node = gap_follow.reactive_node:main' + 'gap_follow_node=gap_follow.gap_follow_node:main', ], }, ) diff --git a/src/gap_follow/src/reactive_node.cpp b/src/gap_follow/src/reactive_node.cpp index 3514ca1..3a42949 100644 --- a/src/gap_follow/src/reactive_node.cpp +++ b/src/gap_follow/src/reactive_node.cpp @@ -36,7 +36,7 @@ class ReactiveFollowGap : public rclcpp::Node // 1.Setting each value to the mean over some window // 2.Rejecting high values (eg. > 3m) this->processed_lidar.clear(); - for (int i = 0; i < scan_msg->ranges.size(); i++) + for ( size_t i = 0; i < scan_msg->ranges.size(); i++) { if (scan_msg->ranges[i] > 3.0) { diff --git a/src/ld_ctrl/CMakeLists.txt b/src/ld_ctrl/CMakeLists.txt new file mode 100644 index 0000000..368c422 --- /dev/null +++ b/src/ld_ctrl/CMakeLists.txt @@ -0,0 +1,15 @@ +cmake_minimum_required(VERSION 3.8) +project(ld_ctrl) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) + +install( + DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +ament_package() \ No newline at end of file diff --git a/src/ld_ctrl/config/slam_toolbox_async.yaml b/src/ld_ctrl/config/slam_toolbox_async.yaml new file mode 100644 index 0000000..169de87 --- /dev/null +++ b/src/ld_ctrl/config/slam_toolbox_async.yaml @@ -0,0 +1,49 @@ +# SLAM Toolbox Configuration for YDLIDAR X2 + +slam_toolbox: + ros__parameters: + use_sim_time: false + + # Plugin stack + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_linear_solver_threads: 1 + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_link + scan_topic: /scan + use_map_saver: true + map_start_at_origin: true + + # If you have odometry (e.g., wheel encoders) + # Set to true and provide /odom topic + use_odometry: false + + # Scan processing + max_beams_to_process: 500 + loop_search_max_chain_size: 5 + loop_match_minimum_chain_size: 10 + do_loop_closing: true + async_corrector_pose_set_topic: /map + + # Constraints + correlation_covariance_scale: 1.0 + loop_closure_transform_threshold: 1.25 + loop_closure_outlier_ratio: 0.08 + loop_closure_outlier_threshold: 2.0 + loop_closure_minimum_chain_size: 10 + + # Minimum scan difference + minimum_scan_hits: 3 + minimum_travel_heading: 0.5 + minimum_travel_distance: 0.5 + map_update_angle: 0.002 + + # Filter doublets + remove_unknown_speckles: true + replenish_frequency: 20.0 diff --git a/src/ld_ctrl/config/ydlidar.yaml b/src/ld_ctrl/config/ydlidar.yaml new file mode 100644 index 0000000..cf9817f --- /dev/null +++ b/src/ld_ctrl/config/ydlidar.yaml @@ -0,0 +1,25 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 115200 + lidar_type: 1 + device_type: 0 + sample_rate: 3 + abnormal_check_count: 4 + fixed_resolution: true + reversion: false + inverted: false + auto_reconnect: true + isSingleChannel: true + intensity_bit: 0 + intensity: false + support_motor_dtr: true + angle_max: 180.0 + angle_min: -180.0 + range_max: 12.0 + range_min: 0.1 + frequency: 10.0 + invalid_range_is_inf: false + debug: false diff --git a/src/ld_ctrl/launch/base_link.launch.py b/src/ld_ctrl/launch/base_link.launch.py new file mode 100644 index 0000000..b9b6d50 --- /dev/null +++ b/src/ld_ctrl/launch/base_link.launch.py @@ -0,0 +1,21 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + """ + Create a dummy base_link frame for the robot + If your car has its own TF broadcaster, skip this + """ + + tf_publisher = Node( + package='tf2_ros', + executable='static_transform_publisher', + arguments=[ + '0.0', '0.0', '0.0', + '0.0', '0.0', '0.0', + 'map', + 'base_link' + ] + ) + + return LaunchDescription([tf_publisher]) diff --git a/src/ld_ctrl/launch/lane_detection.launch.py b/src/ld_ctrl/launch/lane_detection.launch.py new file mode 100644 index 0000000..3549b36 --- /dev/null +++ b/src/ld_ctrl/launch/lane_detection.launch.py @@ -0,0 +1,29 @@ +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +import os + +def generate_launch_description(): + """Launch lane detection pipeline (Ackermann output)""" + + lane_detector = Node( + package='ld_ctrl', + executable='lane_detector_node', + name='lane_detector', + output='screen', + remappings=[ + ('/camera/image_raw', '/camera/image_raw') + ] + ) + + lane_controller = Node( + package='ld_ctrl', + executable='lane_controller_node', + name='lane_controller', + output='screen', + remappings=[ + ('/drive_ackermann', '/drive_ackermann') + ] + ) + + return LaunchDescription([lane_detector, lane_controller]) diff --git a/src/ld_ctrl/launch/ld_ctrl.launch.py b/src/ld_ctrl/launch/ld_ctrl.launch.py new file mode 100644 index 0000000..eb9cc67 --- /dev/null +++ b/src/ld_ctrl/launch/ld_ctrl.launch.py @@ -0,0 +1,37 @@ +from launch import LaunchDescription +from launch_ros.actions import IncludeLaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution + +def generate_launch_description(): + """ + Launch YDLiDAR X2 driver using official launch file + - Includes RViz visualization + - Uses your custom ydlidar.yaml config + """ + + # Get the YDLiDAR driver package location + ydlidar_driver_share = FindPackageShare('ydlidar_ros2_driver') + + # Path to YOUR X2 config file + config_file = PathJoinSubstitution([ + FindPackageShare('ld_ctrl'), # YOUR ld_ctrl package + 'config', + 'ydlidar.yaml' + ]) + + # Include the OFFICIAL YDLiDAR launch file with YOUR config + ydlidar_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + ydlidar_driver_share, + 'launch', + 'ydlidar_launch_view.py' # Official launch + RViz + ]) + ), + launch_arguments={ + 'params_file': config_file # Use YOUR config + }.items() + ) + + return LaunchDescription([ydlidar_launch]) diff --git a/src/ld_ctrl/launch/slam_async.launch.py b/src/ld_ctrl/launch/slam_async.launch.py new file mode 100644 index 0000000..b6b3f37 --- /dev/null +++ b/src/ld_ctrl/launch/slam_async.launch.py @@ -0,0 +1,74 @@ + +from launch import LaunchDescription +from launch_ros.actions import Node, IncludeLaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration + +def generate_launch_description(): + """ + Launch SLAM_Toolbox with YDLIDAR X2 + Creates a real-time map of your environment + """ + + # Declare arguments + use_sim_time = LaunchConfiguration('use_sim_time', default='false') + + # Get SLAM configuration file + slam_config = PathJoinSubstitution([ + FindPackageShare('ld_ctrl'), + 'config', + 'slam_toolbox_async.yaml' + ]) + + ld = LaunchDescription() + + # ============================================ + # 1. Launch LiDAR Driver + # ============================================ + ld_ctrl_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ld_ctrl'), + 'launch', + 'ld_ctrl.launch.py' + ]) + ) + ) + ld.add_action(ld_ctrl_launch) + + # ============================================ + # 2. Static Transform (LiDAR to base_link) + # ============================================ + # This tells ROS where the LiDAR sits on the car + tf_static_publisher = Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_transform_publisher', + output='log', + arguments=[ + '0.0', '0.0', '0.15', # Position: x, y, z (meters) + '0.0', '0.0', '0.0', # Rotation: roll, pitch, yaw (radians) + 'base_link', # Parent frame + 'laser_frame' # Child frame (from ydlidar.yaml) + ] + ) + ld.add_action(tf_static_publisher) + + # ============================================ + # 3. SLAM_Toolbox Node (Async Mode) + # ============================================ + slam_node = Node( + package='slam_toolbox', + executable='async_slam_toolbox_node', + name='slam_toolbox', + output='screen', + parameters=[slam_config], + remappings=[ + ('/scan', '/scan'), + ] + ) + ld.add_action(slam_node) + + return ld diff --git a/src/ld_ctrl/ld_ctrl/__init__.py b/src/ld_ctrl/ld_ctrl/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/ld_ctrl/ld_ctrl/__init__.py @@ -0,0 +1 @@ + diff --git a/src/ld_ctrl/ld_ctrl/lane_controller.py b/src/ld_ctrl/ld_ctrl/lane_controller.py new file mode 100644 index 0000000..830ae04 --- /dev/null +++ b/src/ld_ctrl/ld_ctrl/lane_controller.py @@ -0,0 +1,90 @@ +import rclpy +from rclpy.node import Node +from std_msgs.msg import Float32MultiArray +from ackermann_msgs.msg import AckermannDriveStamped + +class LaneControllerNode(Node): + def __init__(self): + super().__init__('lane_controller_node') + + # PID parameters + self.kp = 1.5 # Proportional gain + self.ki = 0.01 # Integral gain + self.kd = 0.5 # Derivative gain + self.integral = 0.0 + self.prev_error = 0.0 + self.dt = 0.1 + + # Subscribe to lane detection + self.lane_sub = self.create_subscription( + Float32MultiArray, "/lane_detection/lane_info", + self.lane_callback, 10 + ) + + # Publish Ackermann commands (your format) + self.ackermann_pub = self.create_publisher( + AckermannDriveStamped, "/drive_ackermann", 10 + ) + + # Control timer + self.timer = self.create_timer(self.dt, self.control_loop) + self.current_lane = None + + self.get_logger().info('Lane Controller (Ackermann) started') + + def lane_callback(self, msg): + """Process lane detection data""" + if len(msg.data) > 1 and msg.data[0] > 0: + # Average all detected lanes + positions = msg.data[1:] + self.current_lane = sum(positions) / len(positions) + else: + self.current_lane = None + + def control_loop(self): + """PID control loop - outputs Ackermann commands""" + if self.current_lane is None: + # No lanes - send zero steering command + msg = AckermannDriveStamped() + msg.drive.speed = 0.0 + msg.drive.steering_angle = 0.0 + self.ackermann_pub.publish(msg) + return + + # Error = distance from center (0.5 = center of image) + error = self.current_lane - 0.5 + + # PID calculation + self.integral += error * self.dt + derivative = (error - self.prev_error) / self.dt + + # Steering angle (-1.57 to 1.57 radians ~ ±90 degrees) + steering_angle = (self.kp * error + + self.ki * self.integral + + self.kd * derivative) + + # Limit steering angle + steering_angle = max(-1.57, min(1.57, steering_angle)) + + # Publish Ackermann command + msg = AckermannDriveStamped() + msg.drive.speed = 0.3 # Forward speed (m/s) + msg.drive.steering_angle = steering_angle # Radians + msg.drive.steering_angle_velocity = 1.0 # Max turn rate + + self.ackermann_pub.publish(msg) + self.prev_error = error + +def main(args=None): + rclpy.init(args=args) + node = LaneControllerNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/ld_ctrl/ld_ctrl/lane_detector.py b/src/ld_ctrl/ld_ctrl/lane_detector.py new file mode 100644 index 0000000..d2fc1b8 --- /dev/null +++ b/src/ld_ctrl/ld_ctrl/lane_detector.py @@ -0,0 +1,109 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Image +from std_msgs.msg import Float32MultiArray +from cv_bridge import CvBridge +import cv2 +import numpy as np +import os +import yaml + +class LaneDetectorNode(Node): + def __init__(self): + super().__init__('lane_detector_node') + self.bridge = CvBridge() + + # Subscribe to camera + self.image_sub = self.create_subscription( + Image, "/camera/image_raw", self.image_callback, 1 + ) + + # Publish lane info + debug image + self.lane_info_pub = self.create_publisher( + Float32MultiArray, "/lane_detection/lane_info", 10 + ) + self.debug_image_pub = self.create_publisher( + Image, "/lane_detection/debug_image", 10 + ) + self.get_logger().info('Lane Detector started') + + def image_callback(self, msg): + try: + cv_image = self.bridge.imgmsg_to_cv2(msg, "bgr8") + lanes, debug_img = self.detect_lanes(cv_image) + self.publish_lane_info(lanes) + + # Publish debug visualization + debug_msg = self.bridge.cv2_to_imgmsg(debug_img, "bgr8") + self.debug_image_pub.publish(debug_msg) + except Exception as e: + self.get_logger().error(f'Error: {e}') + + def detect_lanes(self, image): + """Detect yellow/white lanes using HSV + Hough""" + debug_image = image.copy() + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + + # Yellow lanes + lower_yellow = np.array([20, 100, 100]) + upper_yellow = np.array([30, 255, 255]) + mask_yellow = cv2.inRange(hsv, lower_yellow, upper_yellow) + + # White lanes + lower_white = np.array([0, 0, 100]) + upper_white = np.array([180, 30, 255]) + mask_white = cv2.inRange(hsv, lower_white, upper_white) + + # Combine masks + combined = cv2.bitwise_or(mask_yellow, mask_white) + + # Clean up noise + kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) + combined = cv2.morphologyEx(combined, cv2.MORPH_CLOSE, kernel) + + # Edge detection + edges = cv2.Canny(combined, 50, 150) + + # Find lane lines + lines = cv2.HoughLinesP(edges, 1, np.pi/180, 50, + minLineLength=50, maxLineGap=10) + + lanes = [] + if lines is not None: + for line in lines: + x1, y1, x2, y2 = line[0] + # Normalized position (0=left, 1=right) + mid_x = (x1 + x2) / 2 + norm_x = mid_x / image.shape[1] + lanes.append(norm_x) + + # Draw line on debug image + cv2.line(debug_image, (x1, y1), (x2, y2), (0, 255, 0), 3) + + # Draw center line + cv2.line(debug_image, (image.shape[1]//2, 0), + (image.shape[1]//2, image.shape[0]), (0, 0, 255), 2) + + return lanes, debug_image + + def publish_lane_info(self, lanes): + msg = Float32MultiArray() + if lanes: + msg.data = [float(len(lanes))] + [float(x) for x in lanes] + else: + msg.data = [0.0] + self.lane_info_pub.publish(msg) + +def main(args=None): + rclpy.init(args=args) + node = LaneDetectorNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/ld_ctrl/package.xml b/src/ld_ctrl/package.xml new file mode 100644 index 0000000..a251ea1 --- /dev/null +++ b/src/ld_ctrl/package.xml @@ -0,0 +1,33 @@ + + + + ld_ctrl + 0.0.1 + Lane Detection + YDLiDAR Control for RC-CAR-ROS2 + Nitish + Apache License 2.0 + + + ament_python + + + rclpy + std_msgs + sensor_msgs + ackermann_msgs + cv_bridge + + + opencv-python + ydlidar_ros2_driver + + + ament_copyright + ament_flake8 + ament_pep257 + python3-pytest + + + ament_python + + diff --git a/src/ld_ctrl/resource/ld_ctrl b/src/ld_ctrl/resource/ld_ctrl new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/ld_ctrl/resource/ld_ctrl @@ -0,0 +1 @@ + diff --git a/src/ld_ctrl/setup.py b/src/ld_ctrl/setup.py new file mode 100644 index 0000000..7bba6fd --- /dev/null +++ b/src/ld_ctrl/setup.py @@ -0,0 +1,39 @@ +from setuptools import find_packages, setup + +package_name = 'ld_ctrl' + +setup( + name=package_name, + version='0.0.1', + packages=find_packages(exclude=['test']), + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', [ + 'launch/ld_ctrl.launch.py', # YDLiDAR driver + 'launch/lane_detection.launch.py', # Lane detection + 'launch/slam_async.launch.py', # SLAM + 'launch/base_link.launch.py', # TF frames + ]), + ('share/' + package_name + '/config', [ + 'config/ydlidar.yaml', + 'config/slam_toolbox_async.yaml', + 'config/lane_detection_config.yaml', # Lane detection config + ]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='Nitish', + maintainer_email='nitish@example.com', + description='Lane Detection + YDLiDAR Control for RC-CAR-ROS2', + license='Apache License 2.0', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'lane_detector_node = ld_ctrl.lane_detector:main', + 'lane_controller_node = ld_ctrl.lane_controller:main', + ], + }, +) + diff --git a/src/llm_ctrl/CMakeList.txt b/src/llm_ctrl/CMakeList.txt new file mode 100644 index 0000000..6c8bf37 --- /dev/null +++ b/src/llm_ctrl/CMakeList.txt @@ -0,0 +1,17 @@ +cmake_minimum_required(VERSION 3.5) +project(llm_ctrl) + +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +ament_package() + + + + + + + + diff --git a/src/llm_ctrl/README.md b/src/llm_ctrl/README.md new file mode 100644 index 0000000..e0c47de --- /dev/null +++ b/src/llm_ctrl/README.md @@ -0,0 +1 @@ +#llm_ctrl diff --git a/src/llm_ctrl/launch/controller.launch.py b/src/llm_ctrl/launch/controller.launch.py new file mode 100755 index 0000000..648e815 --- /dev/null +++ b/src/llm_ctrl/launch/controller.launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='llm_ctrl', + executable='controller', + name='controller_node', + output='screen' + ), + ]) diff --git a/src/llm_ctrl/launch/input.launch.py b/src/llm_ctrl/launch/input.launch.py new file mode 100644 index 0000000..3025dda --- /dev/null +++ b/src/llm_ctrl/launch/input.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch.actions import ExecuteProcess + +def generate_launch_description(): + return LaunchDescription([ + ExecuteProcess( + cmd=['bash', '-c', + 'source /home/driftpilot/RC-CAR-ROS2/whisper/bin/activate && ' + 'python3 /home/driftpilot/RC-CAR-ROS2/src/llm_ctrl/llm_ctrl/input.py'], + name='input_node', + output='screen', + emulate_tty=True + ), + ]) diff --git a/src/llm_ctrl/launch/json_to_ackermann.launch.py b/src/llm_ctrl/launch/json_to_ackermann.launch.py new file mode 100755 index 0000000..4cdef27 --- /dev/null +++ b/src/llm_ctrl/launch/json_to_ackermann.launch.py @@ -0,0 +1,13 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='llm_ctrl', + executable='json_to_ackermann', + name='json_to_ackermann_node', + output='screen' + ), + ]) + diff --git a/src/llm_ctrl/launch/llm_ctrl.launch.py b/src/llm_ctrl/launch/llm_ctrl.launch.py new file mode 100755 index 0000000..bdba51d --- /dev/null +++ b/src/llm_ctrl/launch/llm_ctrl.launch.py @@ -0,0 +1,46 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + + +from launch.actions import OpaqueFunction + +def launch_if_not_running(context): + import subprocess + + try: + output = subprocess.check_output( + ['ros2', 'node', 'list'], + text=True + ) + except Exception: + output = "" + + if '/motor_signals_node' in output: + print('[launch] motor_signals_node already running → skipping launch') + return [Node( + package='llm_ctrl', + executable='json_to_ackermann', + name='json_to_ackermann_node', + output='screen' + )] + + print('[launch] motor_signals_node not running → launching') + return [ + Node( + package='llm_ctrl', + executable='json_to_ackermann', + name='json_to_ackermann_node', + output='screen' + ), + Node( + package='manual_ctrl', + executable='motor_signals', + name='motor_signals_node', + output='screen' + ) + ] + +def generate_launch_description(): + return LaunchDescription([ + OpaqueFunction(function=launch_if_not_running), + ]) diff --git a/src/llm_ctrl/launch/motor_signals.launch.py b/src/llm_ctrl/launch/motor_signals.launch.py new file mode 100644 index 0000000..d8e8a34 --- /dev/null +++ b/src/llm_ctrl/launch/motor_signals.launch.py @@ -0,0 +1,14 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='manual_ctrl', + executable='motor_signals', + name='motor_signals_node', + output='screen' + ), + ]) + + diff --git a/src/llm_ctrl/llm_ctrl/__init__.py b/src/llm_ctrl/llm_ctrl/__init__.py new file mode 100644 index 0000000..8bb0aa3 --- /dev/null +++ b/src/llm_ctrl/llm_ctrl/__init__.py @@ -0,0 +1,11 @@ +self.last_cmd = AckermannDriveStamped() +self.last_cmd.header.frame_id = "base_link" +self.last_update_time = self.get_clock().now() + +self.CONTROL_RATE = 20.0 # Hz +self.TIMEOUT_SEC = 0.3 +self.timer = self.create_timer( + 1.0 / self.CONTROL_RATE, + self.timer_callback +) + diff --git a/src/llm_ctrl/llm_ctrl/controller.py b/src/llm_ctrl/llm_ctrl/controller.py new file mode 100755 index 0000000..9ea6171 --- /dev/null +++ b/src/llm_ctrl/llm_ctrl/controller.py @@ -0,0 +1,191 @@ +#!/usr/bin/env python3 + +import json +import socket +import time +from typing import Dict, Any +from multiprocessing import shared_memory + +import ollama # pip install ollama +import math + +# ================= CONFIG ================= +SHM_NAME = "shm" +SHM_SIZE = 512 + +REMOTE_IP = "172.20.10.9" # RC car IP +REMOTE_PORT = 5000 # RC car port + +MAX_STEERING_RAD = 0.27 + +# ================= PROMPT ================= +SYSTEM_PROMPT = """ +You are a command-to-control translator for an RC car with Ackermann steering. + +You receive a short natural language command from the user. +You MUST respond with ONLY a JSON object (no text, no explanation). + +The JSON schema is strictly: +{ + "speed": float, + "steering_angle": float, + "steering_angle_velocity": float, + "acceleration": float, + "jerk": float +} + +Rules: +- If the user says "set velocity to X", set speed = X (float). +- If no speed is mentioned, keep speed = 0.0. +- If the user says "stop", set speed = 0.0 and steering_angle = 0.0. +- If the user says left/right, do NOT change speed. +- Steering angle is given in DEGREES by the user. +- Convert to radians. +- Clamp steering angle between -0.27 and +0.27 radians. +- acceleration, jerk, steering_angle_velocity = 0.0 unless specified. + +Output ONLY valid JSON. +""" + +RC_KNOWLEDGE = """ +RC Car Commands: +- forward/backward → speed ± +- left/right → steering +- stop → speed=0 steering=0 +- Max speed: 3.0 m/s +""" + +# ===================================================== +class AckermannLLMBridge: + def __init__(self): + # -------- Shared Memory -------- + self.shm = shared_memory.SharedMemory(name=SHM_NAME) + self.shm_buf = self.shm.buf + + # -------- Socket -------- + self.sock = None + self.connect_socket() + + print("🚗 Ackermann LLM Bridge Ready") + print(f"← SHM : {SHM_NAME}") + print(f"→ UDP : {REMOTE_IP}:{REMOTE_PORT}\n") + + # ================= SOCKET ================= + def connect_socket(self): + try: + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + print(f"✅ UDP socket ready ({REMOTE_IP}:{REMOTE_PORT})") + except Exception as e: + print(f"❌ Socket error: {e}") + self.sock = None + + def send_json_to_socket(self, data: Dict[str, Any]): + if not self.sock: + self.connect_socket() + if not self.sock: + return + + try: + msg = json.dumps(data).encode("utf-8") + self.sock.sendto(msg, (REMOTE_IP, REMOTE_PORT)) + print(f"📤 Sent: {msg.decode()}") + except Exception as e: + print(f"❌ Send failed: {e}") + self.sock.close() + self.sock = None + + # ================= SHARED MEMORY ================= + def read_from_shared_memory(self) -> str: + raw = bytes(self.shm_buf[:SHM_SIZE]) + raw = raw.split(b"\x00", 1)[0] + return raw.decode("utf-8", errors="ignore").strip() + + # ================= LLM ================= + def parse_drive_command(self, command: str) -> Dict[str, Any]: + messages = [ + {"role": "system", "content": SYSTEM_PROMPT + "\n\n" + RC_KNOWLEDGE}, + {"role": "user", "content": command}, + ] + + response = ollama.chat( + model="llama3.2:latest", + messages=messages, + format={ + "type": "object", + "properties": { + "speed": {"type": "number"}, + "steering_angle": {"type": "number"}, + "steering_angle_velocity": {"type": "number"}, + "acceleration": {"type": "number"}, + "jerk": {"type": "number"}, + }, + "required": [ + "speed", + "steering_angle", + "steering_angle_velocity", + "acceleration", + "jerk", + ], + }, + ) + + data = json.loads(response["message"]["content"]) + + # ---------- Post-processing ---------- + steering = data.get("steering_angle", 0.0) + + # If steering looks like degrees → convert + if abs(steering) > 1.5: + steering = math.radians(steering) + + # Clamp steering + steering = max(-MAX_STEERING_RAD, min(MAX_STEERING_RAD, steering)) + data["steering_angle"] = steering + + # Defaults + defaults = { + "speed": 0.0, + "steering_angle": 0.0, + "steering_angle_velocity": 0.0, + "acceleration": 0.0, + "jerk": 0.0, + } + defaults.update(data) + + print(f"🤖 LLM Output: {defaults}") + return defaults + + # ================= RUN ================= + def run(self): + last_cmd = "" + + while True: + try: + cmd = self.read_from_shared_memory() + + if cmd and cmd != last_cmd: + print(f"📥 SHM Command: '{cmd}'") + + control = self.parse_drive_command(cmd) + self.send_json_to_socket(control) + + last_cmd = cmd + + time.sleep(0.1) # 10 Hz + + except KeyboardInterrupt: + print("\n👋 Exiting") + break + except Exception as e: + print(f"⚠️ Error: {e}") + time.sleep(0.2) + + +# ================= MAIN ================= +if __name__ == "__main__": + bridge = AckermannLLMBridge() + try: + bridge.run() + finally: + if bridge.sock: + bridge.sock.close() diff --git a/src/llm_ctrl/llm_ctrl/input.py b/src/llm_ctrl/llm_ctrl/input.py new file mode 100644 index 0000000..01d2783 --- /dev/null +++ b/src/llm_ctrl/llm_ctrl/input.py @@ -0,0 +1,211 @@ +#!/usr/bin/env python3 + +import threading +import time +import os +import sys +import json +import numpy as np +import sounddevice as sd +from faster_whisper import WhisperModel +from scipy.signal import butter, filtfilt +from multiprocessing import shared_memory + +# ===== Linux keyboard handling ===== +import termios +import tty +import select + +# ================== OPTIMIZATION ================== +os.environ["OMP_NUM_THREADS"] = "4" +os.environ["CT2_FAST_NATIVE_THREADING"] = "1" + +# ================== CONFIG ================== +SAMPLE_RATE = 16000 +BLOCK_DURATION = 0.1 +CHANNELS = 1 +FRAMES_PER_BLOCK = int(SAMPLE_RATE * BLOCK_DURATION) + +SHM_NAME = "shm" +SHM_SIZE = 512 + +# ================================================== +class TextInputApp: + def __init__(self): + # ---------- Shared Memory ---------- + try: + self.shm = shared_memory.SharedMemory(name=SHM_NAME) + print(f"🔗 Attached to shared memory: {SHM_NAME}") + except FileNotFoundError: + self.shm = shared_memory.SharedMemory( + create=True, size=SHM_SIZE, name=SHM_NAME + ) + print(f"🆕 Created shared memory: {SHM_NAME}") + + self.shm_buf = self.shm.buf + self.shm_lock = threading.Lock() + + # ---------- Audio ---------- + sd.default.samplerate = SAMPLE_RATE + sd.default.channels = CHANNELS + + model = "medium.en" + device = "cpu" + print(f"→ Loading Whisper model: {model} on {device}") + self.model = WhisperModel(model, device=device, compute_type="int8") + + # ---------- State ---------- + self.recording = threading.Event() + self.stop_event = threading.Event() + self.audio_data = [] + self.audio_lock = threading.Lock() + + self.input_mode = 0 # 1 = voice, 2 = keyboard + self.current_text = "" + + print(f"📦 Shared Memory Name : {SHM_NAME}") + print(f"📦 Max Text Size : {SHM_SIZE} bytes") + + # ================== SHARED MEMORY ================== + def write_to_shared_memory(self, text: str): + with self.shm_lock: + self.shm_buf[:] = b"\x00" * SHM_SIZE + data = text.encode("utf-8")[: SHM_SIZE - 1] + self.shm_buf[: len(data)] = data + + # ================== AUDIO ================== + def preprocess_audio(self, audio): + b, a = butter(4, 300, btype="high", fs=SAMPLE_RATE) + return filtfilt(b, a, audio) + + def audio_callback(self, indata, frames, time_, status): + if status: + print(status) + if self.recording.is_set(): + with self.audio_lock: + self.audio_data.extend(indata[:, 0].astype(np.float32)) + + def audio_thread(self): + with sd.InputStream( + channels=CHANNELS, + samplerate=SAMPLE_RATE, + blocksize=FRAMES_PER_BLOCK, + callback=self.audio_callback, + ): + while not self.stop_event.is_set(): + time.sleep(0.1) + + def process_recording(self): + with self.audio_lock: + if len(self.audio_data) < SAMPLE_RATE * 0.5: + print("❌ Audio too short") + return + audio = np.array(self.audio_data[-SAMPLE_RATE * 3:], dtype=np.float32) + + audio = self.preprocess_audio(audio) + audio = np.clip(audio, -1.0, 1.0) + audio /= max(np.max(np.abs(audio)), 1e-6) + + print("🔮 Transcribing...") + segments, _ = self.model.transcribe(audio, language="en", vad_filter=True) + text = " ".join(seg.text.strip() for seg in segments).strip() + + if text: + print(f"✅ Voice → Shared Memory: '{text}'") + self.write_to_shared_memory(text) + else: + print("❌ No speech detected") + + # ================== KEYBOARD (LINUX) ================== + def get_key(self): + dr, _, _ = select.select([sys.stdin], [], [], 0) + if dr: + return sys.stdin.read(1) + return None + + def initial_setup(self): + print("\n=== INPUT MODE ===") + print("1 → 🎤 Voice") + print("2 → ⌨️ Keyboard") + + while self.input_mode == 0: + key = self.get_key() + if key == "1": + self.input_mode = 1 + print("🎤 Voice mode selected") + elif key == "2": + self.input_mode = 2 + print("⌨️ Keyboard mode selected") + time.sleep(0.01) + + def keyboard_thread(self): + print("ESC = toggle mode | Ctrl+C = quit") + + while not self.stop_event.is_set(): + key = self.get_key() + + if key is None: + time.sleep(0.01) + continue + + # ESC + if key == "\x1b": + self.input_mode = 3 - self.input_mode + self.current_text = "" + print( + f"\n🔄 Switched to {'VOICE' if self.input_mode == 1 else 'KEYBOARD'}" + ) + + elif self.input_mode == 1: # VOICE + if key == " ": + print("\n🎤 Recording... ENTER to stop") + self.audio_data.clear() + self.recording.set() + elif key == "\n" and self.recording.is_set(): + self.recording.clear() + self.process_recording() + + else: # KEYBOARD + if key == "\n": + if self.current_text.strip(): + print(f"\n✅ Text → Shared Memory: '{self.current_text}'") + self.write_to_shared_memory(self.current_text) + self.current_text = "" + elif key == "\x7f": # Backspace + self.current_text = self.current_text[:-1] + print("\b \b", end="", flush=True) + elif key.isprintable(): + self.current_text += key + print(key, end="", flush=True) + + # ================== RUN ================== + def run(self): + # Put terminal in raw mode + old_settings = termios.tcgetattr(sys.stdin) + tty.setcbreak(sys.stdin.fileno()) + + try: + self.initial_setup() + threading.Thread(target=self.keyboard_thread, daemon=True).start() + threading.Thread(target=self.audio_thread, daemon=True).start() + + while True: + time.sleep(0.2) + + except KeyboardInterrupt: + print("\n🛑 Exiting...") + + finally: + termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings) + self.stop_event.set() + self.shm.close() + try: + self.shm.unlink() + print("🧹 Shared memory unlinked") + except FileNotFoundError: + pass + + +# ================== MAIN ================== +if __name__ == "__main__": + TextInputApp().run() diff --git a/src/llm_ctrl/llm_ctrl/json_to_ackermann.py b/src/llm_ctrl/llm_ctrl/json_to_ackermann.py new file mode 100755 index 0000000..0b08fa4 --- /dev/null +++ b/src/llm_ctrl/llm_ctrl/json_to_ackermann.py @@ -0,0 +1,112 @@ +import rclpy +from rclpy.node import Node +from ackermann_msgs.msg import AckermannDriveStamped +from std_msgs.msg import Header +from rclpy.qos import QoSProfile + +import socket +import threading +import json + +HOST = "0.0.0.0" +PORT = 5000 + +class LlmAckermannNode(Node): + def __init__(self): + super().__init__("llm_ackermann_node") + + qos = QoSProfile(depth=10) + self.pub = self.create_publisher( + AckermannDriveStamped, + "ackermann_cmd", + qos, + ) + + # Safety limits + self.MAX_SPEED = 3.0 + self.MAX_STEER = 0.6 + self.MAX_ACCEL = 2.0 + self.MAX_JERK = 5.0 + + # UDP socket + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.sock.bind((HOST, PORT)) + + threading.Thread(target=self.udp_loop, daemon=True).start() + self.get_logger().info("LLM Ackermann node listening on UDP 5000") + + def clamp(self, value, min_val, max_val): + return max(min_val, min(max_val, value)) + + def udp_loop(self): + while rclpy.ok(): + try: + data, addr = self.sock.recvfrom(1024) + payload = json.loads(data.decode("utf-8")) + self.publish_command(payload) + except json.JSONDecodeError: + self.get_logger().warn("Invalid JSON received") + except Exception as e: + self.get_logger().error(f"UDP error: {e}") + + def publish_command(self, drive_params: dict): + msg = AckermannDriveStamped() + msg.header = Header() + msg.header.stamp = self.get_clock().now().to_msg() + msg.header.frame_id = "base_link" + + # Extract with defaults for safety + msg.drive.speed = float(drive_params.get("speed", 0.0)) + msg.drive.steering_angle = float(drive_params.get("steering_angle", 0.0)) + msg.drive.steering_angle_velocity = float( + drive_params.get("steering_angle_velocity", 0.0) + ) + msg.drive.acceleration = float(drive_params.get("acceleration", 0.0)) + msg.drive.jerk = float(drive_params.get("jerk", 0.0)) + + # Safety clamping + msg.drive.speed = self.clamp(msg.drive.speed, -self.MAX_SPEED, self.MAX_SPEED) + msg.drive.steering_angle = self.clamp( + msg.drive.steering_angle, -self.MAX_STEER, self.MAX_STEER + ) + msg.drive.acceleration = self.clamp( + msg.drive.acceleration, -self.MAX_ACCEL, self.MAX_ACCEL + ) + msg.drive.jerk = self.clamp(msg.drive.jerk, -self.MAX_JERK, self.MAX_JERK) + + #self.pub.publish(msg) +''' + self.get_logger().info( + f"Published: speed={msg.drive.speed:.2f}, " + f"steer={msg.drive.steering_angle:.2f}" + ) +''' + self.last_cmd = msg + self.last_update_time = self.get_clock.now() + + def timer_callback: + now = self.get_clock().now() + dt = (now - self.last_update_time).nanoseconds * 1e-9 + + # 🚨 Watchdog: stop car if no UDP update + if dt > self.TIMEOUT_SEC: + self.last_cmd.drive.speed = 0.0 + self.last_cmd.drive.steering_angle = 0.0 + + self.last_cmd.header.stamp = now.to_msg() + self.pub.publish(self.last_cmd) + +def main(args=None): + rclpy.init(args=args) + node = LlmAckermannNode() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.destroy_node() + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/llm_ctrl/llm_ctrl/requirements,txt b/src/llm_ctrl/llm_ctrl/requirements,txt new file mode 100644 index 0000000..81fa6bd --- /dev/null +++ b/src/llm_ctrl/llm_ctrl/requirements,txt @@ -0,0 +1,5 @@ +numpy +sounddevice +faster-whisper +scipy +torch diff --git a/src/llm_ctrl/package.xml b/src/llm_ctrl/package.xml new file mode 100644 index 0000000..acda9ee --- /dev/null +++ b/src/llm_ctrl/package.xml @@ -0,0 +1,28 @@ + + + llm_ctrl + 0.0.1 + LLM-based RC car controller using Ollama + llama3.2:1b and AckermannDriveStamped + + Nitish + MIT + + ament_python + + rclpy + ackermann_msgs + std_msgs + ydlidar_ros2_driver + + + gpiozero + numpy + + ament_lint_auto + ament_lint_common + + + ament_python + + + diff --git a/src/llm_ctrl/resource/llm_ctrl b/src/llm_ctrl/resource/llm_ctrl new file mode 100644 index 0000000..e69de29 diff --git a/src/llm_ctrl/scripts/READNE.md b/src/llm_ctrl/scripts/READNE.md new file mode 100644 index 0000000..4e26e8b --- /dev/null +++ b/src/llm_ctrl/scripts/READNE.md @@ -0,0 +1,22 @@ +## 🪟 Windows Setup & Run Instructions + +### 1️⃣ Create and activate virtual environment +```bash +python -m venv whisper +whisper\Scripts\activate +pip install -r requirements.txt +``` + +### terminal 1- input node +```bash +python run input.py +``` + +### Terminal 2 – Controller Module + +> **NOTE:** +> Update the **IP address** in `controller.py` before running. +> Default port: **5000** + +```bash +python run controller.py diff --git a/src/llm_ctrl/scripts/controller.py b/src/llm_ctrl/scripts/controller.py new file mode 100644 index 0000000..0732a25 --- /dev/null +++ b/src/llm_ctrl/scripts/controller.py @@ -0,0 +1,168 @@ +import json +from typing import Dict, Any +import ollama # pip install ollama +import socket +from multiprocessing import shared_memory +import threading +import time + +SHM_NAME = "shm" +SHM_SIZE = 512 +REMOTE_IP = "172.20.10.9" # Change to your RC car's IP +REMOTE_PORT = 5000 # Change if needed + +SYSTEM_PROMPT = """ +You are a command-to-control translator for an RC car with Ackermann steering. + +You receive a short natural language command from the user. +You MUST respond with ONLY a JSON object (no text, no explanation). + +The JSON schema is strictly: +{ + "speed": float, + "steering_angle": float, + "steering_angle_velocity": float, + "acceleration": float, + "jerk": float +} + +Rules: +- If the user says "set velocity to X", set speed = X (float). +- If no speed is mentioned, keep speed = 0.0. +- If no steering command is mentioned, steering_angle = 0.0. +- If the user says "turn left/right", set steering_angle to a small angle: + - "small left/right": +/-0.2 + - "left/right": +/-0.4 + - "sharp left/right": +/-1 +- acceleration, jerk, steering_angle_velocity can be 0.0 unless explicitly mentioned. + +Output ONLY valid JSON, no markdown, no comments, no surrounding text. +""" + +RC_KNOWLEDGE = """ +RC Car Commands: +- "forward/backward" → speed positive/negative +- "left/right turn" → steering_angle -0.4/+0.4 +- "stop" → speed=0, steering=0 +- "drift left" → steering=-0.6, speed=1.5 +- Max speed: 3.0 m/s +""" + +class AckermannLLMBridge: + def __init__(self): + # Connect to shared memory + self.shm = shared_memory.SharedMemory(name=SHM_NAME) + self.shm_buf = self.shm.buf + + # Socket connection + self.sock = None + self.connect_socket() + print("🚗 Ackermann LLM Bridge Ready") + print(f"← Reading from SHM: {SHM_NAME}") + print(f"→ Sending JSON to: {REMOTE_IP}:{REMOTE_PORT}\n") + + def connect_socket(self): + try: + self.sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + self.sock.connect((REMOTE_IP, REMOTE_PORT)) + print(f"✅ Connected to {REMOTE_IP}:{REMOTE_PORT}") + except Exception as e: + print(f"❌ Socket connect failed: {e}") + self.sock = None + + def read_from_shared_memory(self) -> str: + """Read null-terminated string from shared memory""" + raw_bytes = bytes(self.shm_buf[:SHM_SIZE]) + if b"\x00" in raw_bytes: + raw_bytes = raw_bytes.split(b"\x00", 1)[0] + return raw_bytes.decode("utf-8", errors="ignore").strip() + + def parse_drive_command(self, command: str) -> Dict[str, Any]: + messages = [ + {"role": "system", "content": SYSTEM_PROMPT + "\n\nKnowledge: " + RC_KNOWLEDGE}, + {"role": "user", "content": command}, + ] + response = ollama.chat( + model="llama3.2:latest",messages=messages, + format={ + "type": "object", + "properties": { + "speed": {"type": "number"}, + "steering_angle": {"type": "number"}, + "steering_angle_velocity": {"type": "number"}, + "acceleration": {"type": "number"}, + "jerk": {"type": "number"}, + }, + "required": [ + "speed", + "steering_angle", + "steering_angle_velocity", + "acceleration", + "jerk", + ], + }, + ) + + content = response["message"]["content"] + data = json.loads(content) + print(data) + defaults = { + "speed": 0.0, + "steering_angle": 0.0, + "steering_angle_velocity": 0.0, + "acceleration": 0.0, + "jerk": 0.0, + } + defaults.update(data) + return defaults + + def send_json_to_socket(self, data: Dict[str, Any]): + """Send JSON command over socket""" + if not self.sock: + self.connect_socket() + if not self.sock: + return + + try: + json_str = json.dumps(data) + self.sock.sendall(json_str.encode("utf-8") + b"\n") + print(f"📤 Sent: {json_str}") + except Exception as e: + print(f"❌ Socket send failed: {e}") + self.sock.close() + self.sock = None + + def run(self): + last_command = "" + while True: + try: + # Read from shared memory + command = self.read_from_shared_memory() + # print(type(command)) + if command and command != last_command: + print(f"📥 SHM Command: '{command}'") + + # Parse with LLM + result = self.parse_drive_command(command) + + # Send JSON over socket + self.send_json_to_socket(result) + + last_command = command + + time.sleep(0.1) # 10Hz polling + + except KeyboardInterrupt: + print("\n👋 Shutting down...") + break + except Exception as e: + print(f"⚠️ Error: {e}") + time.sleep(0.1) + +if __name__ == "__main__": + bridge = AckermannLLMBridge() + try: + bridge.run() + finally: + if bridge.sock: + bridge.sock.close() diff --git a/src/llm_ctrl/scripts/input.py b/src/llm_ctrl/scripts/input.py new file mode 100644 index 0000000..0f054d5 --- /dev/null +++ b/src/llm_ctrl/scripts/input.py @@ -0,0 +1,208 @@ +#!/usr/bin/env python3 +import threading +import time +import json +import os +import msvcrt +import numpy as np +import sounddevice as sd +from faster_whisper import WhisperModel +from scipy.signal import butter, filtfilt +from multiprocessing import shared_memory +import sys +# # sys.path.insert(0, './') +# import nvidia_cubalas +# import nvidia_cudnn + + +# ================== WINDOWS OPTIMIZATION ================== +os.environ["OMP_NUM_THREADS"] = "4" +os.environ["CT2_FAST_NATIVE_THREADING"] = "1" + +# ================== CONFIG ================== +SAMPLE_RATE = 16000 +BLOCK_DURATION = 0.1 +CHANNELS = 1 +FRAMES_PER_BLOCK = int(SAMPLE_RATE * BLOCK_DURATION) + +SHM_NAME = "shm" +SHM_SIZE = 512 + +# ========================================================== +class TextInputApp: + def __init__(self): + # ---------- Shared Memory ---------- + try: + self.shm = shared_memory.SharedMemory(name=SHM_NAME) + print(f"🔗 Attached to shared memory: {SHM_NAME}") + except FileNotFoundError: + self.shm = shared_memory.SharedMemory( + create=True, size=SHM_SIZE, name=SHM_NAME + ) + print(f"🆕 Created shared memory: {SHM_NAME}") + + self.shm_buf = self.shm.buf + self.shm_lock = threading.Lock() + + # ---------- Audio ---------- + sd.default.samplerate = SAMPLE_RATE + sd.default.channels = CHANNELS + model = "medium.en" + device="cpu" + print(f"->loading model...{model} ..with device {device}") + self.model = WhisperModel(model, device=device, compute_type="int8") + + # ---------- State ---------- + self.recording = threading.Event() + self.stop_event = threading.Event() + self.audio_data = [] + self.audio_lock = threading.Lock() + self.input_mode = 0 # 1 = voice, 2 = keyboard + self.current_text = "" + + print(f"📱 Shared Memory Name : {SHM_NAME}") + print(f"📱 Max Text Size : {SHM_SIZE} bytes") + + # ================== SHARED MEMORY ================== + + """SHM_NAME = "text_input_shm" + SHM_SIZE = 512""" + def write_to_shared_memory(self, text: str): + with self.shm_lock: + # self.shm_buf[:] = b"\x00" * SHM_SIZE + + + self.shm_buf[:SHM_SIZE] = b"0"*(SHM_SIZE) + + data = text.encode("utf-8")[: SHM_SIZE - 1] + if len(data) < SHM_SIZE: + data += b"\x00"*(SHM_SIZE - len(data)) + self.shm_buf[: SHM_SIZE] = data + + # ================== AUDIO ================== + def preprocess_audio(self, audio): + b, a = butter(4, 300, btype="high", fs=SAMPLE_RATE) + return filtfilt(b, a, audio) + + def audio_callback(self, indata, frames, time_, status): + if status: + print(status) + if self.recording.is_set(): + with self.audio_lock: + self.audio_data.extend(indata[:, 0].astype(np.float32)) + + def audio_thread(self): + with sd.InputStream( + channels=CHANNELS, + samplerate=SAMPLE_RATE, + blocksize=FRAMES_PER_BLOCK, + callback=self.audio_callback, + ): + while not self.stop_event.is_set(): + time.sleep(0.1) + + def process_recording(self): + with self.audio_lock: + if len(self.audio_data) < SAMPLE_RATE * 0.5: + print("❌ Audio too short") + return + audio = np.array(self.audio_data[-SAMPLE_RATE * 3:], dtype=np.float32) + + #audio = self.preprocess_audio(audio) + audio = self.preprocess_audio(audio).astype(np.float32) + audio = np.clip(audio, -1.0, 1.0) + audio /= max(np.max(np.abs(audio)), 1e-6) + + print("🔮 Transcribing...") + segments, _ = self.model.transcribe(audio, language="en", vad_filter=True) + text = " ".join(seg.text.strip() for seg in segments).strip() + + if text: + print(f"✅ Voice → Shared Memory: '{text}'") + self.write_to_shared_memory(text) + else: + print("❌ No speech detected") + + # ================== INPUT ================== + def initial_setup(self): + print("\n=== INPUT MODE ===") + print("1 → 🎤 Voice") + print("2 → ⌨️ Keyboard") + + while self.input_mode == 0: + if msvcrt.kbhit(): + k = msvcrt.getch().decode(errors="ignore") + if k == "1": + self.input_mode = 1 + print("🎤 Voice mode selected") + elif k == "2": + self.input_mode = 2 + print("⌨️ Keyboard mode selected") + time.sleep(0.01) + + def keyboard_thread(self): + print("ESC = toggle mode | Ctrl+C = quit") + + while not self.stop_event.is_set(): + if msvcrt.kbhit(): + key = msvcrt.getch() + + # ESC + if key == b"\x1b": + self.input_mode = 3 - self.input_mode + self.current_text = "" + print( + f"\n🔄 Switched to {'VOICE' if self.input_mode==1 else 'KEYBOARD'}" + ) + + elif self.input_mode == 1: # VOICE + if key == b" ": + print("\n🎤 Recording... ENTER to stop") + self.audio_data.clear() + self.recording.set() + elif key == b"\r" and self.recording.is_set(): + self.recording.clear() + self.process_recording() + + else: # KEYBOARD + if key == b"\r": + if self.current_text.strip(): + print(f"\n✅ Text → Shared Memory: '{self.current_text}'") + self.write_to_shared_memory(self.current_text) + self.current_text = "" + elif key == b"\x08": + self.current_text = self.current_text[:-1] + print("\b \b", end="", flush=True) + else: + char = key.decode(errors="ignore") + if char.isprintable(): + self.current_text += char + print(char, end="", flush=True) + + time.sleep(0.01) + + # ================== RUN ================== + def run(self): + self.initial_setup() + + threading.Thread(target=self.keyboard_thread, daemon=True).start() + threading.Thread(target=self.audio_thread, daemon=True).start() + + try: + while True: + time.sleep(0.2) + except KeyboardInterrupt: + print("\n🛑 Exiting...") + finally: + self.shm.close() + try: + self.shm.unlink() + print("🧹 Shared memory unlinked") + except FileNotFoundError: + pass + + + +# ================== MAIN ================== +if __name__ == "__main__": + TextInputApp().run() diff --git a/src/llm_ctrl/scripts/ollama_test b/src/llm_ctrl/scripts/ollama_test new file mode 100644 index 0000000..838f47a --- /dev/null +++ b/src/llm_ctrl/scripts/ollama_test @@ -0,0 +1,3 @@ +import ollama +response = ollama.chat(model='llama3.2:latest', messages=[{'role': 'user', 'content': 'hello'}]) +print(response['message']['content']) diff --git a/src/llm_ctrl/scripts/requirements.txt b/src/llm_ctrl/scripts/requirements.txt new file mode 100644 index 0000000..81fa6bd --- /dev/null +++ b/src/llm_ctrl/scripts/requirements.txt @@ -0,0 +1,5 @@ +numpy +sounddevice +faster-whisper +scipy +torch diff --git a/src/llm_ctrl/scripts/shm.py b/src/llm_ctrl/scripts/shm.py new file mode 100644 index 0000000..ae97863 --- /dev/null +++ b/src/llm_ctrl/scripts/shm.py @@ -0,0 +1,38 @@ +#!/usr/bin/env python3 +import time +from multiprocessing import shared_memory + +SHM_NAME = "shm" +SHM_SIZE = 512 + +def main(): + try: + shm = shared_memory.SharedMemory(name=SHM_NAME) + print(f"✅ Attached to shared memory: {SHM_NAME}") + except FileNotFoundError: + print("❌ Shared memory not found. Start writer first.") + return + + try: + while True: + buf = bytes(shm.buf[:SHM_SIZE]) + + # Raw buffer (hex for debugging) + print("\n--- RAW BUFFER (hex) ---") + print(buf.hex()) + + # Decoded text (null-terminated) + text = buf.split(b"\x00", 1)[0].decode("utf-8", errors="ignore") + print("\n--- DECODED TEXT ---") + print(f"'{text}'") + + time.sleep(0.5) + + except KeyboardInterrupt: + print("\n🛑 Exiting reader") + + finally: + shm.close() + +if __name__ == "__main__": + main() diff --git a/src/llm_ctrl/setup.cfg b/src/llm_ctrl/setup.cfg new file mode 100755 index 0000000..cf8e841 --- /dev/null +++ b/src/llm_ctrl/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/llm_ctrl + +[install] +install_scripts=$base/lib/llm_ctrl diff --git a/src/llm_ctrl/setup.py b/src/llm_ctrl/setup.py new file mode 100755 index 0000000..14e7979 --- /dev/null +++ b/src/llm_ctrl/setup.py @@ -0,0 +1,37 @@ +from setuptools import setup, find_packages + +package_name = 'llm_ctrl' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', [ + 'launch/controller.launch.py', + 'launch/json_to_ackermann.launch.py', + 'launch/llm_ctrl.launch.py', + 'launch/motor_signals.launch.py', + 'launch/input.launch.py', # Include your combined launch file if present + ]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='driftpilot', + maintainer_email='driftpilot@example.com', + description='LLM based controller for RC car', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'controller = llm_ctrl.controller:main', + 'json_to_ackermann = llm_ctrl.json_to_ackermann:main', + 'motor_signals = llm_ctrl.motor_signals:main', + 'input = llm_ctrl.input:main', + ], + }, +) + diff --git a/src/manual_ctrl/CMakeLists.txt b/src/manual_ctrl/CMakeLists.txt new file mode 100644 index 0000000..0c4d0c7 --- /dev/null +++ b/src/manual_ctrl/CMakeLists.txt @@ -0,0 +1,9 @@ +cmake_minimum_required(VERSION 3.5) +project(manual_ctrl) + +find_package(ament_cmake REQUIRED) +find_package(rclpy REQUIRED) + +ament_python_install_package(${PROJECT_NAME}) + +ament_package() diff --git a/src/manual_ctrl/README.md b/src/manual_ctrl/README.md new file mode 100644 index 0000000..bc9cae6 --- /dev/null +++ b/src/manual_ctrl/README.md @@ -0,0 +1,62 @@ +# READ ME + +## Nodes Description + +### 1. `joy_raw` +- Reads raw joystick data from `/joy` device. +- Publishes a ramped, processed joystick message on `/drift_pilot/joy_ramped`. +- Ensures smooth control input translation for further processing. + +### 2. `joy_drive` +- Subscribes to `/drift_pilot/joy_ramped`. +- Converts joystick axes into velocity and steering commands. +- Limits maximum speed and steering angle for safety. +- Publishes `AckermannDriveStamped` commands to steer and move the vehicle. + +### 3. `motor_signals` +- Subscribes to `AckermannDriveStamped` commands. +- Converts drive commands into PWM signals using `gpiozero`. +- Controls GPIO pins for the ESC (speed controller) and the steering servo. +- Handles initialization and safe shutdown with neutral signals. +run the following command to start the pigpiod daemon and the ros2 joystick reades: + +## To launch +```bash +sudo apt update +sudo apt install python3-gpiozero #requires raspi os for controls; runs on pc without control ability +rm -rf install log build +colcon build +source install/setup.bash +ros2 run joy joy_node +``` +then run the launch files + +### 1. manual_ctrl +```bash +ros2 launch manual_ctrl manual_ctrl.launch.py +``` +### 2. joy_raw +```bash +ros2 launch manual_ctrl joy_raw.launch.py +``` +### 3.joy_drive +```bash +ros2 launch manual_ctrl joy_drive.launch.py +``` +### 4. motor_signals +```bash +ros2 launch manual_ctrl motor_signals.launch.py +``` + + +## To run simulation uaing joystick +```bash +#clone the repo +m/f1tenth/f1tenth_gym.git +cd f1tenth_gym + +# install in your virtual environment +pip install -e . + +#launch gymbridge +ros2 launch simulator gym_bridge \ No newline at end of file diff --git a/src/manual_ctrl/launch/joy_drive.launch.py b/src/manual_ctrl/launch/joy_drive.launch.py new file mode 100644 index 0000000..5326cca --- /dev/null +++ b/src/manual_ctrl/launch/joy_drive.launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='manual_ctrl', + executable='joy_drive', + name='joy_drive_node', + output='screen' + ), + ]) diff --git a/src/manual_ctrl/launch/joy_raw.launch.py b/src/manual_ctrl/launch/joy_raw.launch.py new file mode 100644 index 0000000..1ec00e5 --- /dev/null +++ b/src/manual_ctrl/launch/joy_raw.launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='manual_ctrl', + executable='joy_raw', + name='joy_raw_node', + output='screen' + ), + ]) diff --git a/src/manual_ctrl/launch/manual_ctrl.launch.py b/src/manual_ctrl/launch/manual_ctrl.launch.py new file mode 100644 index 0000000..db9662b --- /dev/null +++ b/src/manual_ctrl/launch/manual_ctrl.launch.py @@ -0,0 +1,75 @@ +from launch import LaunchDescription +from launch_ros.actions import Node, IncludeLaunchDescription +from launch_ros.substitutions import FindPackageShare +from launch.substitutions import PathJoinSubstitution +from launch.launch_description_sources import PythonLaunchDescriptionSource + + +def generate_launch_description(): + """ + Main launch file for RC-CAR-ROS2 system. + + Includes: + - Manual Control System + - SLAM Mapping + - Safety Monitor + """ + + ld = LaunchDescription() + + # ============================================ + # MANUAL CONTROL SYSTEM + # ============================================ + joy_raw_node = Node( + package='manual_ctrl', + executable='joy_raw', + name='joy_raw_node', + output='screen' + ) + ld.add_action(joy_raw_node) + + joy_drive_node = Node( + package='manual_ctrl', + executable='joy_drive', + name='joy_drive_node', + output='screen' + ) + ld.add_action(joy_drive_node) + + motor_signals_node = Node( + package='manual_ctrl', + executable='motor_signals', + name='motor_signals_node', + output='screen' + ) + ld.add_action(motor_signals_node) + + # ============================================ + # SLAM MAPPING + # ============================================ + slam_launch = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + PathJoinSubstitution([ + FindPackageShare('ld_ctrl'), + 'launch', + 'slam_async.launch.py' + ]) + ) + ) + ld.add_action(slam_launch) + + # ============================================ + # SAFETY MONITORING NODE + # ============================================ + safety_node = Node( + package='safety_node', + executable='safety_node', + output='screen', + parameters=[{ + 'collision_threshold': 0.3, + 'warning_threshold': 0.5, + }] + ) + ld.add_action(safety_node) + + return ld diff --git a/src/manual_ctrl/launch/motor_signals.launch.py b/src/manual_ctrl/launch/motor_signals.launch.py new file mode 100755 index 0000000..fb6fff2 --- /dev/null +++ b/src/manual_ctrl/launch/motor_signals.launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='manual_ctrl', + executable='motor_signals', + name='motor_signals_node', + output='screen' + ), + ]) diff --git a/src/manual_ctrl/manual_ctrl/Ackerman_sender.py b/src/manual_ctrl/manual_ctrl/Ackerman_sender.py new file mode 100644 index 0000000..58fb761 --- /dev/null +++ b/src/manual_ctrl/manual_ctrl/Ackerman_sender.py @@ -0,0 +1,80 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy +import socket +import json + + +WINDOWS_IP = "192.168.1.100" # 🔁 CHANGE THIS +WINDOWS_PORT = 5005 + + +class SimpleCarController(Node): + def __init__(self): + super().__init__('ackermann_socket_controller') + + # Joystick subscriber (unchanged) + self.joystick_sub = self.create_subscription( + Joy, + 'driftpilot_joy/joy_ramped', + self.joy_callback, + 10 + ) + + # Ackermann state + self.current_speed = 0.0 + self.current_steering = 0.0 + + self.MAX_SPEED = 1.0 + self.MAX_STEERING_ANGLE = 0.52 + + # Socket setup + self.sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + self.sock.connect((WINDOWS_IP, WINDOWS_PORT)) + self.get_logger().info("Connected to Windows socket receiver") + + # 50 Hz update (same as before) + self.timer = self.create_timer(1.0 / 50.0, self.send_commands) + + def joy_callback(self, msg: Joy): + if len(msg.axes) >= 5: + raw_steering_input = msg.axes[2] + raw_throttle_input = msg.axes[1] + + self.current_speed = raw_throttle_input * self.MAX_SPEED + self.current_steering = raw_steering_input * self.MAX_STEERING_ANGLE + else: + self.current_speed = 0.0 + self.current_steering = 0.0 + + def send_commands(self): + ackermann_dict = { + "speed": float(self.current_speed), + "steering_angle": float(self.current_steering), + "steering_angle_velocity": 0.0, + "acceleration": 0.0, + "jerk": 0.0 + } + + payload = json.dumps(ackermann_dict).encode("utf-8") + self.sock.sendall(payload + b"\n") # newline-delimited packets + + +def main(args=None): + rclpy.init(args=args) + node = SimpleCarController() + + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.sock.close() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/src/manual_ctrl/manual_ctrl/Arduino_ide.txt b/src/manual_ctrl/manual_ctrl/Arduino_ide.txt new file mode 100644 index 0000000..a76d39e --- /dev/null +++ b/src/manual_ctrl/manual_ctrl/Arduino_ide.txt @@ -0,0 +1,51 @@ +#include + +Servo esc; +Servo steering; + +const int ESC_PIN = 9; +const int STEERING_PIN = 10; + +float speed = 0.0; +float steering_angle = 0.0; + +void setup() { + Serial.begin(115200); + + esc.attach(ESC_PIN); + steering.attach(STEERING_PIN); + + // Neutral + esc.writeMicroseconds(1000); + steering.writeMicroseconds(1500); + + Serial.println("Arduino ready"); +} + +void loop() { + if (Serial.available()) { + String data = Serial.readStringUntil('\n'); + + int commaIndex = data.indexOf(','); + if (commaIndex > 0) { + speed = data.substring(0, commaIndex).toFloat(); + steering_angle = data.substring(commaIndex + 1).toFloat(); + + // Map speed (-1 to 1) → ESC PWM + int esc_pwm = map(speed * 1000, -1000, 1000, 1000, 2000); + esc_pwm = constrain(esc_pwm, 1000, 2000); + + // Map steering (rad) → servo PWM + int steer_pwm = map(steering_angle * 1000, -520, 520, 1100, 1900); + steer_pwm = constrain(steer_pwm, 1100, 1900); + + esc.writeMicroseconds(esc_pwm); + steering.writeMicroseconds(steer_pwm); + + Serial.print("ESC: "); + Serial.print(esc_pwm); + Serial.print(" | STEER: "); + Serial.println(steer_pwm); + } + } +} \ No newline at end of file diff --git a/src/manual_ctrl/manual_ctrl/__init__.py b/src/manual_ctrl/manual_ctrl/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/manual_ctrl/manual_ctrl/ackermann_receiver.py b/src/manual_ctrl/manual_ctrl/ackermann_receiver.py new file mode 100644 index 0000000..c150838 --- /dev/null +++ b/src/manual_ctrl/manual_ctrl/ackermann_receiver.py @@ -0,0 +1,50 @@ +import socket +import json +import serial +import time + +# ================= SOCKET CONFIG ================= +HOST = "0.0.0.0" +PORT = 5005 + +# ================= ARDUINO SERIAL CONFIG ================= +SERIAL_PORT = "COM6" # Windows: COMx | Linux: /dev/ttyUSB0 +BAUDRATE = 115200 + +# ================= SETUP SERIAL ================= +arduino = serial.Serial(SERIAL_PORT, BAUDRATE, timeout=1) +time.sleep(2) # wait for Arduino reset +print("Arduino connected") + +# ================= SETUP SOCKET ================= +sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) +sock.bind((HOST, PORT)) +sock.listen(1) + +print("Waiting for ROS sender...") +conn, addr = sock.accept() +print(f"Connected from {addr}") + +buffer = "" + +# ================= MAIN LOOP ================= +while True: + data = conn.recv(1024).decode() + if not data: + break + + buffer += data + while "\n" in buffer: + line, buffer = buffer.split("\n", 1) + + ackermann = json.loads(line) + + # Extract values + speed = ackermann["speed"] + steering = ackermann["steering_angle"] + + # Send to Arduino (CSV format) + serial_msg = f"{speed:.3f},{steering:.3f}\n" + arduino.write(serial_msg.encode()) + + print("Sent to Arduino:", serial_msg.strip()) \ No newline at end of file diff --git a/src/manual_ctrl/manual_ctrl/joy_drive.py b/src/manual_ctrl/manual_ctrl/joy_drive.py new file mode 100755 index 0000000..b847a78 --- /dev/null +++ b/src/manual_ctrl/manual_ctrl/joy_drive.py @@ -0,0 +1,150 @@ +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy +from ackermann_msgs.msg import AckermannDriveStamped # Import the specific message + +class SimpleCarController(Node): + def __init__(self): + super().__init__('ackermann_car_controller') + + # New: Publisher for the combined Ackermann drive command + self.drive_pub = self.create_publisher(AckermannDriveStamped, 'ackermann_cmd', 10) + + # Original: Joystick subscriber + self.joystick_sub = self.create_subscription(Joy, 'driftpilot_joy/joy_ramped', self.joy_callback, 10) + + self.current_speed = 0.0 # Corresponds to speed (m/s) + self.current_steering = 0.0 # Corresponds to steering_angle (rad) + + # Define maximum movement values for scaling + self.MAX_SPEED = 1 # Max linear speed in meters/second + self.MAX_STEERING_ANGLE = 0.27 # Max steering angle in radians (approx 30 degrees) + + self.timer = self.create_timer(1.0 / 50.0, self.publish_commands) # 50Hz update + + def joy_callback(self, msg): + # Safety check: Ensure the message has enough axes to read + # Checking for index 4 requires a length of at least 5. + if len(msg.axes) >= 5: + + # --- Input Mapping --- + # Axis 0: Left/Right Stick X-axis (for Steering) + # Axis 4: Trigger/Stick (for Throttle/Speed) - Used based on your original code + + raw_steering_input = msg.axes[2] + raw_throttle_input = msg.axes[1] + + self.MAX_SPEED = 1 # Max linear speed in meters/second + self.MAX_STEERING_ANGLE = 0.52 # Max steering angle in radians (approx 30 degrees) + + # 1. Calculate Speed (Linear): Maps [-1.0, 1.0] input to [-MAX_SPEED, MAX_SPEED] + # Assumes stick up/down maps to forward/backward speed + self.current_speed = (raw_throttle_input) * self.MAX_SPEED + + # 2. Calculate Steering Angle (Angular): Maps [-1.0, 1.0] input to [-MAX_STEERING_ANGLE, MAX_STEERING_ANGLE] + self.current_steering = raw_steering_input * self.MAX_STEERING_ANGLE + else: + # Emergency stop/neutral if the joystick data is invalid + self.current_speed = 0.0 + self.current_steering = 0.0 + + def publish_commands(self): + # 1. Create a new AckermannDriveStamped message + drive_msg = AckermannDriveStamped() + + # 2. Fill the Header + # The 'Stamped' version requires a header for timestamping and frame_id + drive_msg.header.stamp = self.get_clock().now().to_msg() + drive_msg.header.frame_id = 'base_link' # Replace with your robot's base frame + + # 3. Fill the Drive command fields + drive_msg.drive.speed = self.current_speed # Set linear speed (m/s) + drive_msg.drive.steering_angle = self.current_steering # Set steering angle (rad) + + # 4. Publish the command + self.drive_pub.publish(drive_msg) + +def main(args=None): + rclpy.init(args=args) + node = SimpleCarController() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() + + +#!/#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy + +class PS4Controller(Node): + def __init__(self, rate): + super().__init__('joystick_ramped') + + self.subscription = self.create_subscription( + Joy, 'joy', self.joy_callback, 10) + + self.publisher = self.create_publisher( + Joy, 'driftpilot_joy/joy_ramped', 10) + + self.timer = self.create_timer(1.0 / rate, self.publish_joy) + + # ---------- Joy state ---------- + self.target_joy = Joy() + self.target_joy.axes = [0.] * 8 + self.target_joy.buttons = [0] * 11 + + # ---------- Axis mapping ---------- + self.THROTTLE_AXIS = 1 + self.STEERING_AXIS = 2 + + # ---------- Throttle ramp ---------- + self.throttle_target = 0.0 + self.throttle_output = 0.0 + self.THROTTLE_RAMP_RATE = 0.04 # per cycle @ 50 Hz + + def joy_callback(self, msg): + self.target_joy.buttons = msg.buttons + self.target_joy.axes = msg.axes + + if len(msg.axes) > self.THROTTLE_AXIS: + self.throttle_target = msg.axes[self.THROTTLE_AXIS] + else: + self.throttle_target = 0.0 + + def ramp(self, target, current, rate): + delta = target - current + if delta > rate: + delta = rate + elif delta < -rate: + delta = -rate + return current + delta + + def publish_joy(self): + # ---- Apply ramp ONLY to throttle ---- + self.throttle_output = self.ramp( + self.throttle_target, + self.throttle_output, + self.THROTTLE_RAMP_RATE + ) + + self.target_joy.axes[self.THROTTLE_AXIS] = self.throttle_output + self.publisher.publish(self.target_joy) + + +def main(args=None): + rclpy.init(args=args) + joystick = PS4Controller(rate=50) + rclpy.spin(joystick) + joystick.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/manual_ctrl/manual_ctrl/joy_raw.py b/src/manual_ctrl/manual_ctrl/joy_raw.py new file mode 100644 index 0000000..759bc22 --- /dev/null +++ b/src/manual_ctrl/manual_ctrl/joy_raw.py @@ -0,0 +1,69 @@ +#!/#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy + +class PS4Controller(Node): + def __init__(self, rate): + super().__init__('joystick_ramped') + + self.subscription = self.create_subscription( + Joy, 'joy', self.joy_callback, 10) + + self.publisher = self.create_publisher( + Joy, 'driftpilot_joy/joy_ramped', 10) + + self.timer = self.create_timer(1.0 / rate, self.publish_joy) + + # ---------- Joy state ---------- + self.target_joy = Joy() + self.target_joy.axes = [0.] * 8 + self.target_joy.buttons = [0] * 11 + + # ---------- Axis mapping ---------- + self.THROTTLE_AXIS = 1 + self.STEERING_AXIS = 2 + + # ---------- Throttle ramp ---------- + self.throttle_target = 0.0 + self.throttle_output = 0.0 + self.THROTTLE_RAMP_RATE = 0.04 # per cycle @ 50 Hz + + def joy_callback(self, msg): + self.target_joy.buttons = msg.buttons + self.target_joy.axes = msg.axes + + if len(msg.axes) > self.THROTTLE_AXIS: + self.throttle_target = msg.axes[self.THROTTLE_AXIS] + else: + self.throttle_target = 0.0 + + def ramp(self, target, current, rate): + delta = target - current + if delta > rate: + delta = rate + elif delta < -rate: + delta = -rate + return current + delta + + def publish_joy(self): + # ---- Apply ramp ONLY to throttle ---- + self.throttle_output = self.ramp( + self.throttle_target, + self.throttle_output, + self.THROTTLE_RAMP_RATE + ) + + self.target_joy.axes[self.THROTTLE_AXIS] = self.throttle_output + self.publisher.publish(self.target_joy) + + +def main(args=None): + rclpy.init(args=args) + joystick = PS4Controller(rate=50) + rclpy.spin(joystick) + joystick.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/manual_ctrl/manual_ctrl/ld_ctrl_manual.py b/src/manual_ctrl/manual_ctrl/ld_ctrl_manual.py new file mode 100644 index 0000000..06ac336 --- /dev/null +++ b/src/manual_ctrl/manual_ctrl/ld_ctrl_manual.py @@ -0,0 +1,109 @@ +#!/usr/bin/env python3 + +""" +LiDAR Control Node for Manual Mode +Separate node that monitors LiDAR and provides feedback to manual driver. +Publishes warnings and obstacle distances. +Does NOT control the robot - only provides information. +""" + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +import numpy as np + +class LdCtrlManualNode(Node): + def __init__(self): + super().__init__('ld_ctrl_manual') + + # Subscribe to LiDAR scan + self.scan_subscription = self.create_subscription( + LaserScan, + '/scan', + self.scan_callback, + 10 + ) + + # Parameters + self.declare_parameter('warning_threshold', 1.0) # meters + self.declare_parameter('danger_threshold', 0.5) # meters + self.declare_parameter('log_interval', 10) # updates before logging + + self.warning_threshold = self.get_parameter('warning_threshold').value + self.danger_threshold = self.get_parameter('danger_threshold').value + self.log_interval = self.get_parameter('log_interval').value + + self.update_count = 0 + + self.get_logger().info('LiDAR Control Manual Node started') + self.get_logger().info(f'Warning threshold: {self.warning_threshold}m') + self.get_logger().info(f'Danger threshold: {self.danger_threshold}m') + + def scan_callback(self, msg: LaserScan): + """ + Process LiDAR scan and provide feedback to manual driver. + """ + try: + ranges = np.array(msg.ranges) + + # Replace bad values + ranges = np.nan_to_num(ranges, nan=msg.range_max, + posinf=msg.range_max, + neginf=msg.range_max) + + # Get distances in different sectors + left_start = 0 + left_end = int(len(ranges) * 0.25) + front_start = int(len(ranges) * 0.35) + front_end = int(len(ranges) * 0.65) + right_start = int(len(ranges) * 0.75) + right_end = len(ranges) + + left_dist = np.min(ranges[left_start:left_end]) if left_end > left_start else msg.range_max + front_dist = np.min(ranges[front_start:front_end]) if front_end > front_start else msg.range_max + right_dist = np.min(ranges[right_start:right_end]) if right_end > right_start else msg.range_max + + all_min = np.min(ranges[ranges > 0.05]) + + # Increment counter + self.update_count += 1 + + # Log only every N updates to avoid spam + if self.update_count >= self.log_interval: + self.update_count = 0 + + # Check danger levels + if all_min < self.danger_threshold: + self.get_logger().error( + f'🚨 DANGER! Obstacle very close: {all_min:.2f}m\n' + f' Front: {front_dist:.2f}m | Left: {left_dist:.2f}m | Right: {right_dist:.2f}m' + ) + elif all_min < self.warning_threshold: + self.get_logger().warn( + f'⚠️ WARNING! Obstacle approaching: {all_min:.2f}m\n' + f' Front: {front_dist:.2f}m | Left: {left_dist:.2f}m | Right: {right_dist:.2f}m' + ) + else: + self.get_logger().info( + f'✓ Clear. Front: {front_dist:.2f}m | Left: {left_dist:.2f}m | Right: {right_dist:.2f}m' + ) + + except Exception as e: + self.get_logger().error(f'Error in scan callback: {str(e)}') + + +def main(args=None): + rclpy.init(args=args) + ld_ctrl_manual_node = LdCtrlManualNode() + + try: + rclpy.spin(ld_ctrl_manual_node) + except KeyboardInterrupt: + pass + finally: + ld_ctrl_manual_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/manual_ctrl/manual_ctrl/motor_signals.py b/src/manual_ctrl/manual_ctrl/motor_signals.py new file mode 100644 index 0000000..72a4cc4 --- /dev/null +++ b/src/manual_ctrl/manual_ctrl/motor_signals.py @@ -0,0 +1,195 @@ +import rclpy +from rclpy.node import Node +from ackermann_msgs.msg import AckermannDriveStamped +import numpy as np +import time +import threading +import smbus + +# ================= PCA9685 CONFIG ================= +I2C_ADDR = 0x40 +ESC_CH = 0 +SERVO_CH = 1 + +MODE1 = 0x00 +PRESCALE = 0xFE +LED0_ON_L = 0x06 + +# ================= SERVO / ESC PARAMS ================= +SERVO_CENTER_US = 1500 +SERVO_RANGE_US = 200 +MAX_STEERING_RAD = 0.27 # 15.5 deg + +ESC_NEUTRAL_US = 1000 +ESC_FORWARD_MAX_US = 1600 +ESC_CALIB_MAX_US = 2000 +MAX_SPEED_MPS = 1 +STAGE_1 = 1100 +STAGE_2 = 1300 +STAGE_3 = 1500 +PWM_FREQ = 50 # Hz + +# ===================================================== + +class RCCarPWMDriver(Node): + def __init__(self): + super().__init__('rc_car_pwm_driver') + + self.bus = smbus.SMBus(1) + + # 🔧 FIX: init PCA9685 properly + self._init_pca9685() + self.set_pwm_freq(PWM_FREQ) + + # Safe startup + self.set_pwm_us(ESC_CH, ESC_NEUTRAL_US) + self.set_pwm_us(SERVO_CH, SERVO_CENTER_US) + + self.calibrating = True + self.state = 0 + + self.get_logger().info("Starting ESC calibration...") + threading.Thread(target=self._calibrate_esc, daemon=True).start() + + self.subscription = self.create_subscription( + AckermannDriveStamped, + 'ackermann_cmd', + self.drive_callback, + 10 + ) + + # ================= PCA9685 INIT ================= + def _init_pca9685(self): + # Reset + self.bus.write_byte_data(I2C_ADDR, MODE1, 0x00) + time.sleep(0.01) + + # Auto-increment + wake + self.bus.write_byte_data(I2C_ADDR, MODE1, 0x20) + time.sleep(0.01) + + self.get_logger().info("PCA9685 initialized") + + # ================= PCA9685 LOW LEVEL ================= + def write(self, reg, val): + self.bus.write_byte_data(I2C_ADDR, reg, val) + + def set_pwm_freq(self, freq): + prescale = int(25000000 / (4096 * freq) - 1) + oldmode = self.bus.read_byte_data(I2C_ADDR, MODE1) + + self.write(MODE1, (oldmode & 0x7F) | 0x10) # sleep + self.write(PRESCALE, prescale) + self.write(MODE1, oldmode) + time.sleep(0.005) + self.write(MODE1, oldmode | 0x80) + + self.get_logger().info(f"PCA9685 PWM freq set to {freq} Hz") + + def set_pwm_us(self, channel, us): + ticks = int(us * 4096 / 20000) + base = LED0_ON_L + 4 * channel + + self.write(base, 0) # ON_L + self.write(base + 1, 0) # ON_H + self.write(base + 2, ticks & 0xFF) # OFF_L + self.write(base + 3, ticks >> 8) # OFF_H + + # ================= ESC CALIBRATION ================= + def _calibrate_esc(self): + try: + + + self.state = 1 + self.get_logger().info("ESC CAL: NEUTRAL") + self.set_pwm_us(ESC_CH, ESC_NEUTRAL_US) + time.sleep(2) + + self.calibrating = False + self.get_logger().info("✓ ESC calibration complete") + + except Exception as e: + self.get_logger().error(f"ESC calibration error: {e}") + self.calibrating = False + """SERVO_CENTER_US = 1500 + SERVO_RANGE_US = 500 + MAX_STEERING_RAD = 0.27 # 15.5 deg + + ESC_NEUTRAL_US = 1000 + ESC_FORWARD_MAX_US = 1600 + ESC_CALIB_MAX_US = 2000 + MAX_SPEED_MPS = 1""" + # ================= MAPPING ================= + def map_range(self, value, in_min, in_max, out_min, out_max): + #value = np.clip(value, in_min, in_max) + return (value - in_min) * (out_max - out_min)/(in_max-in_min) + out_min + + def convert_speed_to_pwm(self, speed_mps): + #speed_mps = np.clip(speed_mps, 0.0, MAX_SPEED_MPS) + if(abs(speed_mps/MAX_SPEED_MPS) <= 0.1): + return int(self.map_range( + speed_mps, + 0.0, MAX_SPEED_MPS, + ESC_NEUTRAL_US, STAGE_1 + )) + elif(abs(speed_mps/MAX_SPEED_MPS) <= 0.45): + return int(self.map_range( + speed_mps, + 0.0, MAX_SPEED_MPS, + STAGE_1, STAGE_2 + )) + elif (abs(speed_mps/MAX_SPEED_MPS) <= 0.75) : + return int(self.map_range( + speed_mps, + 0.0, MAX_SPEED_MPS, + STAGE_2, STAGE_3 + )) + else: + return int(self.map_range( + speed_mps, + 0.0, MAX_SPEED_MPS, + STAGE_3, ESC_FORWARD_MAX_US + )) + + + def convert_steering_to_pwm(self, angle_rad): + return int(self.map_range( + angle_rad, + -MAX_STEERING_RAD, MAX_STEERING_RAD, + SERVO_CENTER_US - SERVO_RANGE_US, + SERVO_CENTER_US + SERVO_RANGE_US + )) + + # ================= ROS CALLBACK ================= + def drive_callback(self, msg): + if self.calibrating: + return + + speed_pwm = self.convert_speed_to_pwm(msg.drive.speed) + steer_pwm = self.convert_steering_to_pwm(msg.drive.steering_angle) + + self.set_pwm_us(ESC_CH, speed_pwm) + #self.get_logger().info(f"speed-pwm:{speed_pwm}...") + self.set_pwm_us(SERVO_CH, steer_pwm) + self.get_logger().info(f"steer:{steer_pwm}..speed:{speed_pwm}") + + def on_shutdown(self): + self.set_pwm_us(ESC_CH, ESC_NEUTRAL_US) + self.set_pwm_us(SERVO_CH, SERVO_CENTER_US) + time.sleep(0.5) + +# ================= MAIN ================= +def main(args=None): + rclpy.init(args=args) + node = RCCarPWMDriver() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.on_shutdown() + node.destroy_node() + rclpy.shutdown() + +if __name__ == '__main__': + main() diff --git a/src/manual_ctrl/package.xml b/src/manual_ctrl/package.xml new file mode 100644 index 0000000..5018bea --- /dev/null +++ b/src/manual_ctrl/package.xml @@ -0,0 +1,27 @@ + + + manual_ctrl + 0.0.1 + manual control ROS 2 nodes for RC Car (joystick and motor control). + + Nitish + MIT + + ament_python + + rclpy + sensor_msgs + ackermann_msgs + std_msgs + ydlidar_ros2_driver + + gpiozero + numpy + + ament_lint_auto + ament_lint_common + + + ament_python + + diff --git a/src/manual_ctrl/resource/manual_ctrl b/src/manual_ctrl/resource/manual_ctrl new file mode 100644 index 0000000..e69de29 diff --git a/src/manual_ctrl/setup.cfg b/src/manual_ctrl/setup.cfg new file mode 100644 index 0000000..7c9d7a6 --- /dev/null +++ b/src/manual_ctrl/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/manual_ctrl + +[install] +install_scripts=$base/lib/manual_ctrl diff --git a/src/manual_ctrl/setup.py b/src/manual_ctrl/setup.py new file mode 100644 index 0000000..0fe6900 --- /dev/null +++ b/src/manual_ctrl/setup.py @@ -0,0 +1,34 @@ +from setuptools import setup + +package_name = 'manual_ctrl' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', [ + 'launch/joy_raw.launch.py', + 'launch/joy_drive.launch.py', + 'launch/motor_signals.launch.py', + 'launch/manual_ctrl.launch.py', # Include your combined launch file if present + ]), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='YOUR_NAME', + maintainer_email='YOUR_EMAIL@example.com', + description='Manual control nodes for RC Car in ROS2', + license='MIT', + tests_require=['pytest'], + entry_points={ + 'console_scripts': [ + 'joy_raw = manual_ctrl.joy_raw:main', + 'joy_drive = manual_ctrl.joy_drive:main', + 'motor_signals = manual_ctrl.motor_signals:main', + #'ld_ctrl_manual = manual_ctrl.ld_ctrl_manual:main', + ], + }, +) diff --git a/src/oled_eyes/launch/oled_eyes.launch.py b/src/oled_eyes/launch/oled_eyes.launch.py new file mode 100644 index 0000000..9bfd86f --- /dev/null +++ b/src/oled_eyes/launch/oled_eyes.launch.py @@ -0,0 +1,12 @@ +from launch import LaunchDescription +from launch_ros.actions import Node + +def generate_launch_description(): + return LaunchDescription([ + Node( + package='oled_eyes', + executable='oled_eyes', + name='oled_eyes', + output='screen' + ) + ]) diff --git a/src/oled_eyes/oled_eyes/__init__.py b/src/oled_eyes/oled_eyes/__init__.py new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/oled_eyes/oled_eyes/__init__.py @@ -0,0 +1 @@ + diff --git a/src/oled_eyes/oled_eyes/oled_eyes.py b/src/oled_eyes/oled_eyes/oled_eyes.py new file mode 100644 index 0000000..51f9e50 --- /dev/null +++ b/src/oled_eyes/oled_eyes/oled_eyes.py @@ -0,0 +1,350 @@ +"""#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +#from ackermann_msgs.msg import AckermannDrive +from sensor_msgs.msg import Joy +import time +import board +import busio +from adafruit_ssd1306 import SSD1306_I2C +from PIL import Image, ImageDraw +import random + + +class OLEDEyes(Node): + def __init__(self): + super().__init__('oled_eyes_node') + + # --- OLED Setup --- + i2c = busio.I2C(board.SCL, board.SDA) + self.display = SSD1306_I2C(128, 64, i2c, addr=0x3c) + self.display.fill(0) + self.display.show() + self.is_open = True + + # --- Eye parameters --- + self.center_x = 64 + self.center_y = 32 + self.eye_w = 40 + self.eye_h = 25 + self.min_w = 15 + self.min_h = 10 + self.border = 6 + + + self.target_x = self.center_x + self.target_y = self.center_y + self.last_cmd_time = time.time() + self.random_mode = False + self.steering = 0.0 + self.throttle = 0.0 + self.current_scale = 1 + + # ROS2 subscription + #self.create_subscription(AckermannDrive, '/ackermann_cmd', self.callback, 10) + #Joy Subscription + self.create_subscription(Joy, '/joy', self.joy_callback, 10) + + # Timer for updates + self.timer = self.create_timer(0.05, self.update_display) + self.blink_timer = self.create_timer(2.0, self.blink) + self.get_logger().info("OLED Eyes Node Started ✅") + self.reopener = None + self.random_time = self.create_timer(3, self.rand) + def joy_callback(self, msg: Joy): + # Typical joystick mapping (adjust if needed for your controller) + steering_axis = -msg.axes[2] # Left stick horizontal + throttle_axis = msg.axes[1] # Left stick vertical + + # Normalize throttle: Only allow forward response + self.steering = steering_axis # -1 to +1 + self.throttle = max(0.0, throttle_axis) # treat backward as 0 + + self.last_cmd_time = time.time() + self.random_mode = False + + + def draw_eye(self, x, y, w, h): + img = Image.new('1', (128, 64)) + draw = ImageDraw.Draw(img) + x0 = int(x - w / 2) + y0 = int(y - h / 2) + x1 = int(x + w / 2) + y1 = int(y + h / 2) + draw.rounded_rectangle([x0, y0, x1, y1], radius=self.border, fill=255) + self.display.image(img) + self.display.show() + + def rand(self): + if self.random_mode is True: + x = [0, 64, 128] + y = [0, 32, 64] + rand_x = random.choice(x) + rand_y = random.choice(y) + self.target_x = rand_x + self.target_y = rand_y + + def update_display(self): + + self.get_logger().info("In update display\n") + self.draw_eye(self.center_x, self.center_y, self.eye_w, self.eye_h) + if self.throttle ==0 and self.steering == 0: + now = time.time() + flag = True + while(time.time()-now<3): + pass + # Random motion every 2–3 sec if idle + self.random_mode =False + + while(self.throttle ==0 and self.steering == 0): + # Smooth transition + self.center_x += (self.target_x - self.center_x) * 0.15 + self.center_y += (self.target_y - self.center_y) * 0.1 + self.draw_eye(self.center_x, self.center_y, self.eye_w, self.eye_h) + if not self.random_mode: + self.get_logger().info("Entering random motion mode") + self.random_mode = True + target_x = self.target_x + target_y = self.target_y + else: + target_x = 64 + target_y = 32 + else: + target_x = 64 + target_y = 32 + + half_w = self.eye_w / 2 + if self.steering != 0: + target_x = self.center_x + (128 - self.eye_w) * (abs(self.steering) / self.steering) + else: + if self.random_mode is False: + target_x = 64 + + half_w = self.eye_w / 2 + self.center_x = max(half_w, min(self.center_x, 128 - half_w)) + + # Smooth transition + self.center_x += (target_x - self.center_x) * 0.15 + self.center_y += (target_y - self.center_y) * 0.1 + + # Shrink based on throttle + +# Called every frame/update + target_scale = 1.0 if self.throttle == 0 else (0.5 - self.throttle / 2) + +# Smooth factor: smaller = slower, larger = faster transition + smooth_speed = 0.1 + +# Linear interpolation toward target scale + self.current_scale = ( + self.current_scale + + (target_scale - self.current_scale) * smooth_speed + ) + + self.eye_w = 40 * self.current_scale + self.eye_h = 25 * self.current_scale + + + self.draw_eye(self.center_x, self.center_y, self.eye_w, self.eye_h) + + def shutdown(self): + """"""Clean shutdown - clear the OLED display"""""" + self.get_logger().info("Shutting down OLED Eyes... 👋") + self.display.fill(0) + self.display.show() + self.get_logger().info("OLED cleared ✅") + + def blink(self): + if self.is_open: + self.display.fill(0) + self.display.show() + self.is_open = False + else: + self.draw_eye(self.center_x, self.center_y, self.eye_w, self.eye_h) + self.is_open = True + + +def main(args=None): + rclpy.init(args=args) + node = OLEDEyes() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.shutdown() # ← Clear display on exit + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main()""" + + + +#!/usr/bin/env python3 +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy +import time +import board +import busio +from adafruit_ssd1306 import SSD1306_I2C +from PIL import Image, ImageDraw +import random + + +class OLEDEyes(Node): + def __init__(self): + super().__init__('oled_eyes_node') + + # ---------- OLED ---------- + i2c = busio.I2C(board.SCL, board.SDA) + self.display = SSD1306_I2C(128, 64, i2c, addr=0x3c) + self.display.fill(0) + self.display.show() + + # ---------- EYE PARAMS ---------- + self.center_x = 64 + self.center_y = 32 + self.eye_w = 40 + self.eye_h = 25 + self.border = 6 + + self.current_scale = 1.0 + self.target_x = 64 + self.target_y = 32 + + # ---------- INPUT STATE ---------- + self.steering = 0.0 + self.throttle = 0.0 + + self.prev_steering = 0.0 + self.prev_throttle = 0.0 + + self.last_change_time = time.time() + self.random_mode = False + + self.DEADZONE = 0.05 + self.IDLE_TIME = 3.0 + + # ---------- ROS ---------- + self.create_subscription(Joy, '/joy', self.joy_callback, 10) + + self.create_timer(0.05, self.update_display) + self.create_timer(3.0, self.random_target) + self.create_timer(2.0, self.blink) + + self.is_open = True + self.get_logger().info("OLED Eyes Node Started ✅") + + # ================= CALLBACKS ================= + + def joy_callback(self, msg: Joy): + steering = -msg.axes[2] + throttle = max(0.0, msg.axes[1]) + + steering_changed = abs(steering - self.prev_steering) > self.DEADZONE + throttle_changed = abs(throttle - self.prev_throttle) > self.DEADZONE + + if steering_changed or throttle_changed: + self.last_change_time = time.time() + self.random_mode = False + + self.steering = steering + self.throttle = throttle + + self.prev_steering = steering + self.prev_throttle = throttle + + def random_target(self): + if time.time() - self.last_change_time > self.IDLE_TIME and self.steering == 0 and self.throttle == 0: + if not self.random_mode: + self.get_logger().info("Entering RANDOM mode 👀") + self.random_mode = True + + self.target_x = random.randint(20, 108) + self.target_y = random.randint(16, 48) + + # ================= DRAW ================= + + def draw_eye(self): + img = Image.new('1', (128, 64)) + draw = ImageDraw.Draw(img) + + x0 = int(self.center_x - self.eye_w / 2) + y0 = int(self.center_y - self.eye_h / 2) + x1 = int(self.center_x + self.eye_w / 2) + y1 = int(self.center_y + self.eye_h / 2) + + draw.rounded_rectangle( + [x0, y0, x1, y1], + radius=self.border, + fill=255 + ) + + self.display.image(img) + self.display.show() + + # ================= UPDATE LOOP ================= + + def update_display(self): + # ---- TARGET SELECTION ---- + if not self.random_mode: + if abs(self.steering) > self.DEADZONE: + self.target_x = 64 + (128 - self.eye_w) * self.steering + else: + self.target_x = 64 + + self.target_y = 32 + + # ---- SMOOTH MOTION ---- + self.center_x += (self.target_x - self.center_x) * 0.15 + self.center_y += (self.target_y - self.center_y) * 0.10 + + self.center_x = max(10, min(118, self.center_x)) + self.center_y = max(10, min(54, self.center_y)) + + # ---- THROTTLE → SQUINT ---- + target_scale = 1.0 if self.throttle == 0 else 0.5 + self.current_scale += (target_scale - self.current_scale) * 0.1 + + self.eye_w = 40 * self.current_scale + self.eye_h = 25 * self.current_scale + + self.draw_eye() + + # ================= BLINK ================= + + def blink(self): + if self.is_open: + self.display.fill(0) + self.display.show() + else: + self.draw_eye() + + self.is_open = not self.is_open + + # ================= SHUTDOWN ================= + + def shutdown(self): + self.display.fill(0) + self.display.show() + + +def main(args=None): + rclpy.init(args=args) + node = OLEDEyes() + try: + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + node.shutdown() + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() + diff --git a/src/oled_eyes/package.xml b/src/oled_eyes/package.xml new file mode 100644 index 0000000..30537f2 --- /dev/null +++ b/src/oled_eyes/package.xml @@ -0,0 +1,18 @@ + + + oled_eyes + 0.0.1 + OLED eyes visualization node for Ackermann car + + Manoj + MIT + + rclpy + ackermann_msgs + + python3-adafruit-ssd1306 + python3-pil + + ament_python + + diff --git a/src/oled_eyes/resource/oled_eyes b/src/oled_eyes/resource/oled_eyes new file mode 100644 index 0000000..e69de29 diff --git a/src/oled_eyes/setup.cfg b/src/oled_eyes/setup.cfg new file mode 100644 index 0000000..d6ba10d --- /dev/null +++ b/src/oled_eyes/setup.cfg @@ -0,0 +1,5 @@ +[develop] +script_dir=$base/lib/oled_eyes + +[install] +install_scripts=$base/lib/oled_eyes diff --git a/src/oled_eyes/setup.py b/src/oled_eyes/setup.py new file mode 100644 index 0000000..804a843 --- /dev/null +++ b/src/oled_eyes/setup.py @@ -0,0 +1,26 @@ +from setuptools import setup + +package_name = 'oled_eyes' + +setup( + name=package_name, + version='0.0.1', + packages=[package_name], + data_files=[ + ('share/ament_index/resource_index/packages', + ['resource/' + package_name]), + ('share/' + package_name, ['package.xml']), + ('share/' + package_name + '/launch', ['launch/oled_eyes.launch.py']), + ], + install_requires=['setuptools'], + zip_safe=True, + maintainer='ubuntu', + maintainer_email='ubuntu@todo.todo', + description='OLED eyes node for displaying emotions or direction', + license='MIT', + entry_points={ + 'console_scripts': [ + 'oled_eyes = oled_eyes.oled_eyes:main' + ], + }, +) diff --git a/src/safety_node/package.xml b/src/safety_node/package.xml index 7d0828e..6b1bc4b 100644 --- a/src/safety_node/package.xml +++ b/src/safety_node/package.xml @@ -16,6 +16,7 @@ sensor_msgs ackermann_msgs nav_msgs + ydlidar_ros2_driver ament_lint_auto ament_lint_common diff --git a/src/safety_node/safety_node/safety_node.py b/src/safety_node/safety_node/safety_node.py new file mode 100644 index 0000000..84e214b --- /dev/null +++ b/src/safety_node/safety_node/safety_node.py @@ -0,0 +1,128 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import Twist +import numpy as np + +class SafetyNode(Node): + def __init__(self): + super().__init__('safety_node') + + # Subscribe to LiDAR scan + self.scan_subscription = self.create_subscription( + LaserScan, + '/scan', + self.scan_callback, + 10 + ) + + # Subscribe to commanded velocity + self.cmd_subscription = self.create_subscription( + Twist, + '/cmd_vel', + self.cmd_callback, + 10 + ) + + # Publish safe velocity (after safety checks) + self.safe_velocity_publisher = self.create_publisher( + Twist, + '/safe_cmd_vel', + 10 + ) + + # Parameters + self.declare_parameter('collision_threshold', 0.3) # 30cm + self.declare_parameter('warning_threshold', 0.5) # 50cm + self.declare_parameter('front_angle_range', 90) # degrees + + self.collision_threshold = self.get_parameter('collision_threshold').value + self.warning_threshold = self.get_parameter('warning_threshold').value + self.front_angle_range = self.get_parameter('front_angle_range').value + + self.last_cmd = Twist() + self.emergency_stop = False + + self.get_logger().info('Safety Node started') + self.get_logger().info(f'Collision threshold: {self.collision_threshold}m') + + def cmd_callback(self, msg: Twist): + """Store last velocity command.""" + self.last_cmd = msg + + def scan_callback(self, msg: LaserScan): + """ + Monitor LiDAR scan for obstacles. + Prevent collisions by issuing emergency stop if needed. + """ + try: + ranges = np.array(msg.ranges) + + # Replace bad values + ranges = np.nan_to_num(ranges, nan=msg.range_max, + posinf=msg.range_max, + neginf=msg.range_max) + + # Check front sector for collisions + angle_range_rad = np.radians(self.front_angle_range / 2) + front_start_idx = int(len(ranges) * + (msg.angle_min + angle_range_rad) / + (msg.angle_max - msg.angle_min)) + front_end_idx = int(len(ranges) * + (msg.angle_max - angle_range_rad) / + (msg.angle_max - msg.angle_min)) + + front_ranges = ranges[max(0, front_start_idx):min(len(ranges), front_end_idx)] + min_front_distance = np.min(front_ranges) if len(front_ranges) > 0 else msg.range_max + + # Check all sectors + min_distance = np.min(ranges[ranges > 0.05]) # Ignore very close noise + + # Decision logic + if min_distance < self.collision_threshold or min_front_distance < self.collision_threshold: + # COLLISION IMMINENT - HARD STOP + self.emergency_stop = True + safe_cmd = Twist() # All zeros = stop + self.get_logger().error( + f'⚠️ EMERGENCY STOP! Min dist: {min_distance:.2f}m' + ) + elif min_distance < self.warning_threshold: + # WARNING - reduce speed but allow some movement + self.emergency_stop = False + safe_cmd = Twist() + safe_cmd.linear.x = max(0, self.last_cmd.linear.x * 0.3) # 30% speed + safe_cmd.angular.z = self.last_cmd.angular.z * 0.5 # 50% turn + self.get_logger().warn( + f'⚠️ WARNING! Min dist: {min_distance:.2f}m - reducing speed' + ) + else: + # SAFE - allow normal command + self.emergency_stop = False + safe_cmd = self.last_cmd + + self.safe_velocity_publisher.publish(safe_cmd) + + except Exception as e: + self.get_logger().error(f'Error in scan callback: {str(e)}') + # Failsafe: stop on error + safe_cmd = Twist() + self.safe_velocity_publisher.publish(safe_cmd) + + +def main(args=None): + rclpy.init(args=args) + safety_node = SafetyNode() + + try: + rclpy.spin(safety_node) + except KeyboardInterrupt: + pass + finally: + safety_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/safety_node/setup.py b/src/safety_node/setup.py index 7ea4ddd..05d8433 100644 --- a/src/safety_node/setup.py +++ b/src/safety_node/setup.py @@ -4,23 +4,11 @@ setup( name=package_name, - version='0.0.0', packages=[package_name], - data_files=[ - ('share/ament_index/resource_index/packages', - ['resource/' + package_name]), - ('share/' + package_name, ['package.xml']), - ], install_requires=['setuptools'], - zip_safe=True, - maintainer='OKHADIR Hamza', - maintainer_email='hamza.okhadir2018@gmail.com', - description='My solution for the Automatic Emergency Braking System based on TTC calculation', - license='MIT', - tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'safety_node = safety_node.safety_node:main', + 'safety_node=safety_node.safety_node:main', ], }, ) diff --git a/src/simulator/README.md b/src/simulator/README.md index 21c421a..c54d513 100644 --- a/src/simulator/README.md +++ b/src/simulator/README.md @@ -70,3 +70,31 @@ ros2 run teleop_twist_keyboard teleop_twist_keyboard ``` Then, press `i` to move forward, `u` and `o` to move forward and turn, `,` to move backwards, `m` and `.` to move backwards and turn, and `k` to stop in the terminal window running the teleop node. + + +# updated gym bridge + +```bash +# Install ROS2 dependencies +sudo apt update +sudo apt install -y \ + ros-humble-rclpy \ + ros-humble-sensor-msgs \ + ros-humble-nav-msgs \ + ros-humble-geometry-msgs \ + ros-humble-ackermann-msgs \ + ros-humble-tf2-ros \ + ros-humble-robot-state-publisher \ + ros-humble-joint-state-publisher \ + ros-humble-xacro \ + ros-humble-rviz2 + +# Navigate to your package directory +cd /path/to/your/workspace/src/simulator + +# Install Python requirements +pip install -r requirements.txt + +# Or install with pip3 +pip3 install -r requirements.txt +``` \ No newline at end of file diff --git a/src/simulator/config/sim.yaml b/src/simulator/config/sim.yaml index fbd97fc..9db03c8 100644 --- a/src/simulator/config/sim.yaml +++ b/src/simulator/config/sim.yaml @@ -1,62 +1,108 @@ -# MIT License +# # MIT License -# Copyright (c) 2024 S7our Squad +# # Copyright (c) 2024 S7our Squad -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: +# # Permission is hereby granted, free of charge, to any person obtaining a copy +# # of this software and associated documentation files (the "Software"), to deal +# # in the Software without restriction, including without limitation the rights +# # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# # copies of the Software, and to permit persons to whom the Software is +# # furnished to do so, subject to the following conditions: -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. +# # The above copyright notice and this permission notice shall be included in all +# # copies or substantial portions of the Software. -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. +# # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# # SOFTWARE. -bridge: +# bridge: +# ros__parameters: +# # topics and namespaces +# ego_namespace: 'ego_racecar' +# ego_scan_topic: 'scan' +# ego_odom_topic: 'odom' +# ego_opp_odom_topic: 'opp_odom' +# ego_drive_topic: 'drive' +# opp_namespace: 'opp_racecar' +# opp_scan_topic: 'opp_scan' +# opp_odom_topic: 'odom' +# opp_ego_odom_topic: 'opp_odom' +# opp_drive_topic: 'opp_drive' + +# # transform related +# scan_distance_to_base_link: 0.0 + +# # laserscan parameters +# scan_fov: 4.7 +# scan_beams: 1080 + +# # map parameters +# map_path: '/home/goldenhawk/ros2_ws/RC-CAR-ROS2/src/simulator/maps/Budapest_map' +# map_img_ext: '.png' + +# # opponent parameters +# num_agent: 1 + +# # ego starting pose on map +# sx: 0.0 +# sy: 0.0 +# stheta: 0.0 + +# # opp starting pose on map +# sx1: 2.0 +# sy1: 0.5 +# stheta1: 0.0 + +# # teleop +# kb_teleop: True + + + +# config/sim.yaml +# Configuration file for F1TENTH Gym Bridge Simulator + +gym_bridge: ros__parameters: - # topics and namespaces + # Ego vehicle configuration ego_namespace: 'ego_racecar' - ego_scan_topic: 'scan' ego_odom_topic: 'odom' ego_opp_odom_topic: 'opp_odom' + ego_scan_topic: 'scan' ego_drive_topic: 'drive' + + # Opponent vehicle configuration (if num_agent = 2) opp_namespace: 'opp_racecar' - opp_scan_topic: 'opp_scan' opp_odom_topic: 'odom' - opp_ego_odom_topic: 'opp_odom' - opp_drive_topic: 'opp_drive' - - # transform related - scan_distance_to_base_link: 0.0 + opp_ego_odom_topic: 'ego_odom' + opp_scan_topic: 'scan' + opp_drive_topic: 'drive' - # laserscan parameters - scan_fov: 4.7 + # LIDAR configuration + scan_distance_to_base_link: 0.275 + scan_fov: 4.7 # radians (~270 degrees) scan_beams: 1080 - - # map parameters - map_path: '/sim_ws/src/simulator/maps/Budapest_map' + + # Map configuration (will be overridden by launch argument) + map_path: 'levine' map_img_ext: '.png' - - # opponent parameters + + # Number of agents (1 or 2) num_agent: 1 - - # ego starting pose on map + + # Initial pose for ego vehicle (Levine map) sx: 0.0 sy: 0.0 stheta: 0.0 - - # opp starting pose on map + + # Initial pose for opponent vehicle (if num_agent = 2) sx1: 2.0 - sy1: 0.5 + sy1: 0.0 stheta1: 0.0 - - # teleop - kb_teleop: True \ No newline at end of file + + # Enable keyboard teleop + kb_teleop: true \ No newline at end of file diff --git a/src/simulator/launch/ego_racecar.xacro b/src/simulator/launch/ego_racecar.xacro index 39e4456..92854a4 100644 --- a/src/simulator/launch/ego_racecar.xacro +++ b/src/simulator/launch/ego_racecar.xacro @@ -7,6 +7,7 @@ + diff --git a/src/simulator/launch/gym_bridge_launch.py b/src/simulator/launch/gym_bridge_launch.py index c919b07..2e283c5 100644 --- a/src/simulator/launch/gym_bridge_launch.py +++ b/src/simulator/launch/gym_bridge_launch.py @@ -1,94 +1,162 @@ -# MIT License - -# Copyright (c) 2024 S7our Squad -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: - -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. - -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - +#!/usr/bin/env python3 +import yaml from launch import LaunchDescription from launch_ros.actions import Node -from launch.substitutions import Command +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration, Command +from launch_ros.substitutions import FindPackageShare +from launch.conditions import IfCondition +from launch.substitutions import PythonExpression from ament_index_python.packages import get_package_share_directory import os -import yaml + def generate_launch_description(): - ld = LaunchDescription() + # Get package directory + # pkg_share = FindPackageShare('simulator').find('simulator') + pkg_share = get_package_share_directory('simulator') + + + # Paths to files + ego_xacro_file = os.path.join(pkg_share, 'launch', 'ego_racecar.xacro') + rviz_config_file = os.path.join(pkg_share, 'launch', 'gym_bridge.rviz') + params_file = os.path.join(pkg_share, 'config', 'sim.yaml') + default_map = os.path.join(pkg_share, 'maps', 'levine') + config = os.path.join( - get_package_share_directory('simulator'), - 'config', - 'sim.yaml' - ) + get_package_share_directory('simulator'), + 'config', + 'sim.yaml' + ) config_dict = yaml.safe_load(open(config, 'r')) - has_opp = config_dict['bridge']['ros__parameters']['num_agent'] > 1 - teleop = config_dict['bridge']['ros__parameters']['kb_teleop'] + has_opp = config_dict['gym_bridge']['ros__parameters']['num_agent'] > 1 + teleop = config_dict['gym_bridge']['ros__parameters']['kb_teleop'] + - bridge_node = Node( + # Declare launch arguments + map_path_arg = DeclareLaunchArgument( + 'map_path', + default_value=default_map, + description='Path to the map file (without extension)' + ) + + num_agent_arg = DeclareLaunchArgument( + 'num_agent', + default_value='1', + description='Number of agents (1 or 2)' + ) + + use_rviz_arg = DeclareLaunchArgument( + 'use_rviz', + default_value='true', + description='Whether to start RViz' + ) + + # Get launch configurations + map_path = LaunchConfiguration('map_path') + num_agent = LaunchConfiguration('num_agent') + use_rviz = LaunchConfiguration('use_rviz') + + # Process xacro to generate URDF for ego vehicle + # Note: ego_robot_description is only needed if used later, but Command() is sufficient + # for direct use in robot_state_publisher. + + # Gym Bridge Node + gym_bridge_node = Node( package='simulator', executable='gym_bridge', - name='bridge', - parameters=[config] - ) - rviz_node = Node( - package='rviz2', - executable='rviz2', - name='rviz', - arguments=['-d', os.path.join(get_package_share_directory('simulator'), 'launch', 'gym_bridge.rviz')] + name='gym_bridge', + output='screen', + parameters=[ + params_file, + { + 'map_path': map_path, + 'num_agent': num_agent, + } + ], + emulate_tty=True, ) + + # Map Server Node to publish the OccupancyGrid to /map topic map_server_node = Node( package='nav2_map_server', executable='map_server', - parameters=[{'yaml_filename': config_dict['bridge']['ros__parameters']['map_path'] + '.yaml'}, - {'topic': 'map'}, - {'frame_id': 'map'}, - {'output': 'screen'}, - {'use_sim_time': True}] + name='map_server', + output='screen', + parameters=[ + { + # FIX: Use a list to concatenate LaunchConfiguration and string literal + # 'yaml_filename': [map_path, '.yaml'], + + 'yaml_filename': PythonExpression(["'", map_path, ".yaml'"]), + + 'topic_name': 'map', + 'frame_id': 'map', + 'use_sim_time': False + } + ] ) - nav_lifecycle_node = Node( + + # ADDED: Lifecycle Manager to activate the map_server node (Required for Nav2 nodes) + lifecycle_manager_node = Node( package='nav2_lifecycle_manager', executable='lifecycle_manager', name='lifecycle_manager_localization', output='screen', - parameters=[{'use_sim_time': True}, - {'autostart': True}, - {'node_names': ['map_server']}] + parameters=[ + {'use_sim_time': False}, + {'autostart': True}, + {'node_names': ['map_server']} # This manages the node named 'map_server' + ] ) - ego_robot_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='ego_robot_state_publisher', - parameters=[{'robot_description': Command(['xacro ', os.path.join(get_package_share_directory('simulator'), 'launch', 'ego_racecar.xacro')])}], - remappings=[('/robot_description', 'ego_robot_description')] + + + # Robot State Publisher for ego vehicle + robot_state_publisher_ego= Node( + package='robot_state_publisher', + executable='robot_state_publisher', + name='ego_robot_state_publisher', + parameters=[{'robot_description': Command(['xacro ', os.path.join(get_package_share_directory('simulator'), 'launch', 'ego_racecar.xacro')])}], + remappings=[('/robot_description', 'ego_robot_description')] ) - opp_robot_publisher = Node( - package='robot_state_publisher', - executable='robot_state_publisher', - name='opp_robot_state_publisher', - parameters=[{'robot_description': Command(['xacro ', os.path.join(get_package_share_directory('simulator'), 'launch', 'opp_racecar.xacro')])}], - remappings=[('/robot_description', 'opp_robot_description')] + + # Joint State Publisher for ego (publishes static transforms) + joint_state_publisher_ego = Node( + package='joint_state_publisher', + executable='joint_state_publisher', + name='joint_state_publisher', + namespace='ego_racecar', + parameters=[{ + 'use_sim_time': False, + }], + ) + + # RViz Node + rviz_node = Node( + package='rviz2', + executable='rviz2', + name='rviz2', + arguments=['-d', rviz_config_file], + output='screen', + condition=IfCondition(use_rviz), + env={ + 'LIBGL_ALWAYS_SOFTWARE': '1' + } ) - # finalize - ld.add_action(rviz_node) - ld.add_action(bridge_node) - ld.add_action(nav_lifecycle_node) - ld.add_action(map_server_node) - ld.add_action(ego_robot_publisher) - if has_opp: - ld.add_action(opp_robot_publisher) - - return ld \ No newline at end of file + + return LaunchDescription([ + # Arguments + map_path_arg, + num_agent_arg, + use_rviz_arg, + + # Nodes + gym_bridge_node, + map_server_node, + lifecycle_manager_node, # <-- CRITICAL: Activates the map_server + robot_state_publisher_ego, + # robot_state_publisher_opp, # Uncomment for 2 agents + joint_state_publisher_ego, + rviz_node, + ]) diff --git a/src/simulator/package.xml b/src/simulator/package.xml index 8935aa8..ec695d6 100644 --- a/src/simulator/package.xml +++ b/src/simulator/package.xml @@ -1,32 +1,31 @@ - simulator - 0.0.0 - ROS2 simulator package for our RC Car project. - Hamza OKHADIR + 1.0.0 + Custom ROS 2 F1TENTH simulator bridge and visualization stack. + + Your Name MIT + + ament_python + + + rclpy + nav2_map_server + nav2_lifecycle_manager + robot_state_publisher + joint_state_publisher + rviz2 + ackermann_msgs geometry_msgs - nav_msgs sensor_msgs - ackermann_msgs + std_msgs launch launch_ros - tf2_ros - rclpy - joint_state_publisher - xacro - nav2_map_server - nav2_lifecycle_manager - teleop_twist_keyboard - - python-transforms3d-pip - ament_copyright - ament_flake8 - ament_pep257 - python3-pytest + + tf2_ros ament_python diff --git a/src/simulator/requirements.txt b/src/simulator/requirements.txt new file mode 100644 index 0000000..5773870 --- /dev/null +++ b/src/simulator/requirements.txt @@ -0,0 +1,15 @@ +# Python Requirements for F1TENTH Gym Bridge +# Install with: pip install -r requirements.txt + +# Core dependencies +numpy>=1.21.0 +scipy>=1.9.0 +pillow>=9.0.0 +pyyaml>=6.0 + +# Performance optimization (optional but recommended) +numba>=0.56.0 + +# Note: ROS2 packages should be installed via apt: +# sudo apt install ros-humble-rclpy ros-humble-sensor-msgs ros-humble-nav-msgs +# sudo apt install ros-humble-geometry-msgs ros-humble-ackermann-msgs ros-humble-tf2-ros \ No newline at end of file diff --git a/src/simulator/setup.py b/src/simulator/setup.py index 99efc9e..5f78e5c 100644 --- a/src/simulator/setup.py +++ b/src/simulator/setup.py @@ -1,32 +1,36 @@ from setuptools import setup -import os from glob import glob +import os package_name = 'simulator' setup( name=package_name, - version='0.0.0', + version='1.0.0', packages=[package_name], data_files=[ ('share/ament_index/resource_index/packages', ['resource/' + package_name]), ('share/' + package_name, ['package.xml']), + # Launch files (os.path.join('share', package_name, 'launch'), glob('launch/*.py')), (os.path.join('share', package_name, 'launch'), glob('launch/*.xacro')), (os.path.join('share', package_name, 'launch'), glob('launch/*.rviz')), + # Config files (os.path.join('share', package_name, 'config'), glob('config/*.yaml')), + # Map files + (os.path.join('share', package_name, 'maps'), glob('maps/*')), ], install_requires=['setuptools'], zip_safe=True, - maintainer='Hamza OKHADIR ', - maintainer_email='hamza.okhadir2018@gmail.com', - description='ROS2 simulator package for our RC Car project.', + maintainer='Your Name', + maintainer_email='your.email@example.com', + description='F1TENTH Gym Bridge Simulator', license='MIT', tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'gym_bridge = simulator.gym_bridge:main' + 'gym_bridge = simulator.gym_bridge:main', ], }, -) +) \ No newline at end of file diff --git a/src/simulator/simulator/gym_bridge.py b/src/simulator/simulator/gym_bridge.py index 58cae3d..5ba86cd 100644 --- a/src/simulator/simulator/gym_bridge.py +++ b/src/simulator/simulator/gym_bridge.py @@ -1,411 +1,122 @@ -# MIT License - -# Copyright (c) 2024 S7our Squad - -# Permission is hereby granted, free of charge, to any person obtaining a copy -# of this software and associated documentation files (the "Software"), to deal -# in the Software without restriction, including without limitation the rights -# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell -# copies of the Software, and to permit persons to whom the Software is -# furnished to do so, subject to the following conditions: - -# The above copyright notice and this permission notice shall be included in all -# copies or substantial portions of the Software. - -# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR -# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, -# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE -# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER -# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, -# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE -# SOFTWARE. - +#!/usr/bin/env python3 import rclpy from rclpy.node import Node -from sensor_msgs.msg import LaserScan +import numpy as np +import math + from nav_msgs.msg import Odometry -from geometry_msgs.msg import PoseStamped -from geometry_msgs.msg import PoseWithCovarianceStamped -from geometry_msgs.msg import Twist -from geometry_msgs.msg import TransformStamped -from geometry_msgs.msg import Transform -from geometry_msgs.msg import Quaternion +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import TransformStamped, Quaternion from ackermann_msgs.msg import AckermannDriveStamped from tf2_ros import TransformBroadcaster +from tf_transformations import quaternion_from_euler -import gym -import numpy as np -from transforms3d import euler -class GymBridge(Node): - def __init__(self): - super().__init__('gym_bridge') +class SimpleSimulator: + def __init__(self, dt=0.02): + self.dt = dt + self.wheelbase = 0.33 - self.declare_parameter('ego_namespace') - self.declare_parameter('ego_odom_topic') - self.declare_parameter('ego_opp_odom_topic') - self.declare_parameter('ego_scan_topic') - self.declare_parameter('ego_drive_topic') - self.declare_parameter('opp_namespace') - self.declare_parameter('opp_odom_topic') - self.declare_parameter('opp_ego_odom_topic') - self.declare_parameter('opp_scan_topic') - self.declare_parameter('opp_drive_topic') - self.declare_parameter('scan_distance_to_base_link') - self.declare_parameter('scan_fov') - self.declare_parameter('scan_beams') - self.declare_parameter('map_path') - self.declare_parameter('map_img_ext') - self.declare_parameter('num_agent') - self.declare_parameter('sx') - self.declare_parameter('sy') - self.declare_parameter('stheta') - self.declare_parameter('sx1') - self.declare_parameter('sy1') - self.declare_parameter('stheta1') - self.declare_parameter('kb_teleop') + self.x = 0.0 + self.y = 0.0 + self.theta = 0.0 - # check num_agents - num_agents = self.get_parameter('num_agent').value - if num_agents < 1 or num_agents > 2: - raise ValueError('num_agents should be either 1 or 2.') - elif not isinstance(num_agents, int): - raise ValueError('num_agents should be an int.') + self.v = 0.0 + self.delta = 0.0 - # env backend - self.env = gym.make('f110_gym:f110-v0', - map=self.get_parameter('map_path').value, - map_ext=self.get_parameter('map_img_ext').value, - num_agents=num_agents) + def step(self): + if abs(self.v) > 1e-3: + omega = self.v * math.tan(self.delta) / self.wheelbase + else: + omega = 0.0 - sx = self.get_parameter('sx').value - sy = self.get_parameter('sy').value - stheta = self.get_parameter('stheta').value - self.ego_pose = [sx, sy, stheta] - self.ego_speed = [0.0, 0.0, 0.0] - self.ego_requested_speed = 0.0 - self.ego_steer = 0.0 - self.ego_collision = False - ego_scan_topic = self.get_parameter('ego_scan_topic').value - ego_drive_topic = self.get_parameter('ego_drive_topic').value - scan_fov = self.get_parameter('scan_fov').value - scan_beams = self.get_parameter('scan_beams').value - self.angle_min = -scan_fov / 2. - self.angle_max = scan_fov / 2. - self.angle_inc = scan_fov / scan_beams - self.ego_namespace = self.get_parameter('ego_namespace').value - ego_odom_topic = self.ego_namespace + '/' + self.get_parameter('ego_odom_topic').value - self.scan_distance_to_base_link = self.get_parameter('scan_distance_to_base_link').value - - if num_agents == 2: - self.has_opp = True - self.opp_namespace = self.get_parameter('opp_namespace').value - sx1 = self.get_parameter('sx1').value - sy1 = self.get_parameter('sy1').value - stheta1 = self.get_parameter('stheta1').value - self.opp_pose = [sx1, sy1, stheta1] - self.opp_speed = [0.0, 0.0, 0.0] - self.opp_requested_speed = 0.0 - self.opp_steer = 0.0 - self.opp_collision = False - self.obs, _ , self.done, _ = self.env.reset(np.array([[sx, sy, stheta], [sx1, sy1, stheta1]])) - self.ego_scan = list(self.obs['scans'][0]) - self.opp_scan = list(self.obs['scans'][1]) + self.theta += omega * self.dt + self.theta = math.atan2(math.sin(self.theta), math.cos(self.theta)) - opp_scan_topic = self.get_parameter('opp_scan_topic').value - opp_odom_topic = self.opp_namespace + '/' + self.get_parameter('opp_odom_topic').value - opp_drive_topic = self.get_parameter('opp_drive_topic').value + self.x += self.v * math.cos(self.theta) * self.dt + self.y += self.v * math.sin(self.theta) * self.dt - ego_opp_odom_topic = self.ego_namespace + '/' + self.get_parameter('ego_opp_odom_topic').value - opp_ego_odom_topic = self.opp_namespace + '/' + self.get_parameter('opp_ego_odom_topic').value - else: - self.has_opp = False - self.obs, _ , self.done, _ = self.env.reset(np.array([[sx, sy, stheta]])) - self.ego_scan = list(self.obs['scans'][0]) + return self.x, self.y, self.theta, self.v, omega - # sim physical step timer - self.drive_timer = self.create_timer(0.01, self.drive_timer_callback) - # topic publishing timer - self.timer = self.create_timer(0.004, self.timer_callback) - # transform broadcaster - self.br = TransformBroadcaster(self) +class GymBridge(Node): + def __init__(self): + super().__init__('simple_sim_bridge') - # publishers - self.ego_scan_pub = self.create_publisher(LaserScan, ego_scan_topic, 10) - self.ego_odom_pub = self.create_publisher(Odometry, ego_odom_topic, 10) - self.ego_drive_published = False - if num_agents == 2: - self.opp_scan_pub = self.create_publisher(LaserScan, opp_scan_topic, 10) - self.ego_opp_odom_pub = self.create_publisher(Odometry, ego_opp_odom_topic, 10) - self.opp_odom_pub = self.create_publisher(Odometry, opp_odom_topic, 10) - self.opp_ego_odom_pub = self.create_publisher(Odometry, opp_ego_odom_topic, 10) - self.opp_drive_published = False + self.sim = SimpleSimulator() - # subscribers - self.ego_drive_sub = self.create_subscription( + self.odom_pub = self.create_publisher(Odometry, '/odom', 10) + self.scan_pub = self.create_publisher(LaserScan, '/scan', 10) + self.drive_sub = self.create_subscription( AckermannDriveStamped, - ego_drive_topic, + '/drive', self.drive_callback, - 10) - self.ego_reset_sub = self.create_subscription( - PoseWithCovarianceStamped, - '/initialpose', - self.ego_reset_callback, - 10) - if num_agents == 2: - self.opp_drive_sub = self.create_subscription( - AckermannDriveStamped, - opp_drive_topic, - self.opp_drive_callback, - 10) - self.opp_reset_sub = self.create_subscription( - PoseStamped, - '/goal_pose', - self.opp_reset_callback, - 10) + 10 + ) - if self.get_parameter('kb_teleop').value: - self.teleop_sub = self.create_subscription( - Twist, - '/cmd_vel', - self.teleop_callback, - 10) + self.tf_br = TransformBroadcaster(self) + self.timer = self.create_timer(0.02, self.update) - def drive_callback(self, drive_msg): - self.ego_requested_speed = drive_msg.drive.speed - self.ego_steer = drive_msg.drive.steering_angle - self.ego_drive_published = True + def drive_callback(self, msg): + self.sim.v = msg.drive.speed + self.sim.delta = msg.drive.steering_angle - def opp_drive_callback(self, drive_msg): - self.opp_requested_speed = drive_msg.drive.speed - self.opp_steer = drive_msg.drive.steering_angle - self.opp_drive_published = True + def update(self): + x, y, theta, v, omega = self.sim.step() + now = self.get_clock().now().to_msg() - def ego_reset_callback(self, pose_msg): - rx = pose_msg.pose.pose.position.x - ry = pose_msg.pose.pose.position.y - rqx = pose_msg.pose.pose.orientation.x - rqy = pose_msg.pose.pose.orientation.y - rqz = pose_msg.pose.pose.orientation.z - rqw = pose_msg.pose.pose.orientation.w - _, _, rtheta = euler.quat2euler([rqw, rqx, rqy, rqz], axes='sxyz') - if self.has_opp: - opp_pose = [self.obs['poses_x'][1], self.obs['poses_y'][1], self.obs['poses_theta'][1]] - self.obs, _ , self.done, _ = self.env.reset(np.array([[rx, ry, rtheta], opp_pose])) - else: - self.obs, _ , self.done, _ = self.env.reset(np.array([[rx, ry, rtheta]])) - - def opp_reset_callback(self, pose_msg): - if self.has_opp: - rx = pose_msg.pose.position.x - ry = pose_msg.pose.position.y - rqx = pose_msg.pose.orientation.x - rqy = pose_msg.pose.orientation.y - rqz = pose_msg.pose.orientation.z - rqw = pose_msg.pose.orientation.w - _, _, rtheta = euler.quat2euler([rqw, rqx, rqy, rqz], axes='sxyz') - self.obs, _ , self.done, _ = self.env.reset(np.array([list(self.ego_pose), [rx, ry, rtheta]])) - def teleop_callback(self, twist_msg): - if not self.ego_drive_published: - self.ego_drive_published = True - - self.ego_requested_speed = twist_msg.linear.x - - if twist_msg.angular.z > 0.0: - self.ego_steer = 0.3 - elif twist_msg.angular.z < 0.0: - self.ego_steer = -0.3 - else: - self.ego_steer = 0.0 - - def drive_timer_callback(self): - if self.ego_drive_published and not self.has_opp: - self.obs, _, self.done, _ = self.env.step(np.array([[self.ego_steer, self.ego_requested_speed]])) - elif self.ego_drive_published and self.has_opp and self.opp_drive_published: - self.obs, _, self.done, _ = self.env.step(np.array([[self.ego_steer, self.ego_requested_speed], [self.opp_steer, self.opp_requested_speed]])) - self._update_sim_state() + # -------- Odometry (odom -> base_link) -------- + odom = Odometry() + odom.header.stamp = now + odom.header.frame_id = 'odom' + odom.child_frame_id = 'base_link' - def timer_callback(self): - ts = self.get_clock().now().to_msg() - - # pub scans - scan = LaserScan() - scan.header.stamp = ts - scan.header.frame_id = self.ego_namespace + '/laser' - scan.angle_min = self.angle_min - scan.angle_max = self.angle_max - scan.angle_increment = self.angle_inc - scan.range_min = 0. - scan.range_max = 30. - scan.ranges = self.ego_scan - self.ego_scan_pub.publish(scan) + odom.pose.pose.position.x = x + odom.pose.pose.position.y = y - if self.has_opp: - opp_scan = LaserScan() - opp_scan.header.stamp = ts - opp_scan.header.frame_id = self.opp_namespace + '/laser' - opp_scan.angle_min = self.angle_min - opp_scan.angle_max = self.angle_max - opp_scan.angle_increment = self.angle_inc - opp_scan.range_min = 0. - opp_scan.range_max = 30. - opp_scan.ranges = self.opp_scan - self.opp_scan_pub.publish(opp_scan) + q = quaternion_from_euler(0.0, 0.0, theta) + odom.pose.pose.orientation = Quaternion( + x=q[0], y=q[1], z=q[2], w=q[3] + ) - # pub tf - self._publish_odom(ts) - self._publish_transforms(ts) - self._publish_laser_transforms(ts) - self._publish_wheel_transforms(ts) + odom.twist.twist.linear.x = v + odom.twist.twist.angular.z = omega - def _update_sim_state(self): - self.ego_scan = list(self.obs['scans'][0]) - if self.has_opp: - self.opp_scan = list(self.obs['scans'][1]) - self.opp_pose[0] = self.obs['poses_x'][1] - self.opp_pose[1] = self.obs['poses_y'][1] - self.opp_pose[2] = self.obs['poses_theta'][1] - self.opp_speed[0] = self.obs['linear_vels_x'][1] - self.opp_speed[1] = self.obs['linear_vels_y'][1] - self.opp_speed[2] = self.obs['ang_vels_z'][1] + self.odom_pub.publish(odom) - self.ego_pose[0] = self.obs['poses_x'][0] - self.ego_pose[1] = self.obs['poses_y'][0] - self.ego_pose[2] = self.obs['poses_theta'][0] - self.ego_speed[0] = self.obs['linear_vels_x'][0] - self.ego_speed[1] = self.obs['linear_vels_y'][0] - self.ego_speed[2] = self.obs['ang_vels_z'][0] + # -------- TF (odom -> base_link) -------- + tf = TransformStamped() + tf.header.stamp = now + tf.header.frame_id = 'odom' + tf.child_frame_id = 'base_link' + tf.transform.translation.x = x + tf.transform.translation.y = y + tf.transform.rotation = odom.pose.pose.orientation - + self.tf_br.sendTransform(tf) - def _publish_odom(self, ts): - ego_odom = Odometry() - ego_odom.header.stamp = ts - ego_odom.header.frame_id = 'map' - ego_odom.child_frame_id = self.ego_namespace + '/base_link' - ego_odom.pose.pose.position.x = self.ego_pose[0] - ego_odom.pose.pose.position.y = self.ego_pose[1] - ego_quat = euler.euler2quat(0., 0., self.ego_pose[2], axes='sxyz') - ego_odom.pose.pose.orientation.x = ego_quat[1] - ego_odom.pose.pose.orientation.y = ego_quat[2] - ego_odom.pose.pose.orientation.z = ego_quat[3] - ego_odom.pose.pose.orientation.w = ego_quat[0] - ego_odom.twist.twist.linear.x = self.ego_speed[0] - ego_odom.twist.twist.linear.y = self.ego_speed[1] - ego_odom.twist.twist.angular.z = self.ego_speed[2] - self.ego_odom_pub.publish(ego_odom) - - if self.has_opp: - opp_odom = Odometry() - opp_odom.header.stamp = ts - opp_odom.header.frame_id = 'map' - opp_odom.child_frame_id = self.opp_namespace + '/base_link' - opp_odom.pose.pose.position.x = self.opp_pose[0] - opp_odom.pose.pose.position.y = self.opp_pose[1] - opp_quat = euler.euler2quat(0., 0., self.opp_pose[2], axes='sxyz') - opp_odom.pose.pose.orientation.x = opp_quat[1] - opp_odom.pose.pose.orientation.y = opp_quat[2] - opp_odom.pose.pose.orientation.z = opp_quat[3] - opp_odom.pose.pose.orientation.w = opp_quat[0] - opp_odom.twist.twist.linear.x = self.opp_speed[0] - opp_odom.twist.twist.linear.y = self.opp_speed[1] - opp_odom.twist.twist.angular.z = self.opp_speed[2] - self.opp_odom_pub.publish(opp_odom) - self.opp_ego_odom_pub.publish(ego_odom) - self.ego_opp_odom_pub.publish(opp_odom) - - def _publish_transforms(self, ts): - ego_t = Transform() - ego_t.translation.x = self.ego_pose[0] - ego_t.translation.y = self.ego_pose[1] - ego_t.translation.z = 0.0 - ego_quat = euler.euler2quat(0.0, 0.0, self.ego_pose[2], axes='sxyz') - ego_t.rotation.x = ego_quat[1] - ego_t.rotation.y = ego_quat[2] - ego_t.rotation.z = ego_quat[3] - ego_t.rotation.w = ego_quat[0] - - ego_ts = TransformStamped() - ego_ts.transform = ego_t - ego_ts.header.stamp = ts - ego_ts.header.frame_id = 'map' - ego_ts.child_frame_id = self.ego_namespace + '/base_link' - self.br.sendTransform(ego_ts) - - if self.has_opp: - opp_t = Transform() - opp_t.translation.x = self.opp_pose[0] - opp_t.translation.y = self.opp_pose[1] - opp_t.translation.z = 0.0 - opp_quat = euler.euler2quat(0.0, 0.0, self.opp_pose[2], axes='sxyz') - opp_t.rotation.x = opp_quat[1] - opp_t.rotation.y = opp_quat[2] - opp_t.rotation.z = opp_quat[3] - opp_t.rotation.w = opp_quat[0] - - opp_ts = TransformStamped() - opp_ts.transform = opp_t - opp_ts.header.stamp = ts - opp_ts.header.frame_id = 'map' - opp_ts.child_frame_id = self.opp_namespace + '/base_link' - self.br.sendTransform(opp_ts) - - def _publish_wheel_transforms(self, ts): - ego_wheel_ts = TransformStamped() - ego_wheel_quat = euler.euler2quat(0., 0., self.ego_steer, axes='sxyz') - ego_wheel_ts.transform.rotation.x = ego_wheel_quat[1] - ego_wheel_ts.transform.rotation.y = ego_wheel_quat[2] - ego_wheel_ts.transform.rotation.z = ego_wheel_quat[3] - ego_wheel_ts.transform.rotation.w = ego_wheel_quat[0] - ego_wheel_ts.header.stamp = ts - ego_wheel_ts.header.frame_id = self.ego_namespace + '/front_left_hinge' - ego_wheel_ts.child_frame_id = self.ego_namespace + '/front_left_wheel' - self.br.sendTransform(ego_wheel_ts) - ego_wheel_ts.header.frame_id = self.ego_namespace + '/front_right_hinge' - ego_wheel_ts.child_frame_id = self.ego_namespace + '/front_right_wheel' - self.br.sendTransform(ego_wheel_ts) + # -------- Fake LaserScan (for TF testing only) -------- + scan = LaserScan() + scan.header.stamp = now + scan.header.frame_id = 'laser' + scan.angle_min = -2.35 + scan.angle_max = 2.35 + scan.angle_increment = 4.7 / 1080 + scan.range_min = 0.1 + scan.range_max = 30.0 + scan.ranges = [10.0] * 1080 # ⚠️ fake - if self.has_opp: - opp_wheel_ts = TransformStamped() - opp_wheel_quat = euler.euler2quat(0., 0., self.opp_steer, axes='sxyz') - opp_wheel_ts.transform.rotation.x = opp_wheel_quat[1] - opp_wheel_ts.transform.rotation.y = opp_wheel_quat[2] - opp_wheel_ts.transform.rotation.z = opp_wheel_quat[3] - opp_wheel_ts.transform.rotation.w = opp_wheel_quat[0] - opp_wheel_ts.header.stamp = ts - opp_wheel_ts.header.frame_id = self.opp_namespace + '/front_left_hinge' - opp_wheel_ts.child_frame_id = self.opp_namespace + '/front_left_wheel' - self.br.sendTransform(opp_wheel_ts) - opp_wheel_ts.header.frame_id = self.opp_namespace + '/front_right_hinge' - opp_wheel_ts.child_frame_id = self.opp_namespace + '/front_right_wheel' - self.br.sendTransform(opp_wheel_ts) + self.scan_pub.publish(scan) - def _publish_laser_transforms(self, ts): - ego_scan_ts = TransformStamped() - ego_scan_ts.transform.translation.x = self.scan_distance_to_base_link - # ego_scan_ts.transform.translation.z = 0.04+0.1+0.025 - ego_scan_ts.transform.rotation.w = 1. - ego_scan_ts.header.stamp = ts - ego_scan_ts.header.frame_id = self.ego_namespace + '/base_link' - ego_scan_ts.child_frame_id = self.ego_namespace + '/laser' - self.br.sendTransform(ego_scan_ts) - if self.has_opp: - opp_scan_ts = TransformStamped() - opp_scan_ts.transform.translation.x = self.scan_distance_to_base_link - opp_scan_ts.transform.rotation.w = 1. - opp_scan_ts.header.stamp = ts - opp_scan_ts.header.frame_id = self.opp_namespace + '/base_link' - opp_scan_ts.child_frame_id = self.opp_namespace + '/laser' - self.br.sendTransform(opp_scan_ts) +def main(): + rclpy.init() + node = GymBridge() + rclpy.spin(node) + rclpy.shutdown() -def main(args=None): - rclpy.init(args=args) - gym_bridge = GymBridge() - rclpy.spin(gym_bridge) if __name__ == '__main__': main() diff --git a/src/simulator/simulator/sim_core.py b/src/simulator/simulator/sim_core.py new file mode 100644 index 0000000..e69de29 diff --git a/src/wall_follow/package.xml b/src/wall_follow/package.xml index 3677f7a..5bcb139 100644 --- a/src/wall_follow/package.xml +++ b/src/wall_follow/package.xml @@ -16,6 +16,7 @@ sensor_msgs ackermann_msgs geometry_msgs + ydlidar_ros2_driver ament_lint_auto ament_lint_common diff --git a/src/wall_follow/wall_follow/wall_follow_node.py b/src/wall_follow/wall_follow/wall_follow_node.py new file mode 100644 index 0000000..b741b99 --- /dev/null +++ b/src/wall_follow/wall_follow/wall_follow_node.py @@ -0,0 +1,115 @@ +#!/usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import LaserScan +from geometry_msgs.msg import Twist +import math +import numpy as np + +class WallFollowNode(Node): + def __init__(self): + super().__init__('wall_follow') + + # Subscribe to LiDAR scan + self.scan_subscription = self.create_subscription( + LaserScan, + '/scan', + self.scan_callback, + 10 + ) + + # Publish velocity commands + self.velocity_publisher = self.create_publisher( + Twist, + '/cmd_vel', + 10 + ) + + # Parameters + self.declare_parameter('target_distance', 0.5) # meters from wall + self.declare_parameter('linear_velocity', 0.3) + self.declare_parameter('kp', 1.0) # Proportional gain + self.declare_parameter('kd', 0.1) # Derivative gain + self.declare_parameter('wall_side', 'left') # left or right + + self.target_distance = self.get_parameter('target_distance').value + self.linear_velocity = self.get_parameter('linear_velocity').value + self.kp = self.get_parameter('kp').value + self.kd = self.get_parameter('kd').value + self.wall_side = self.get_parameter('wall_side').value + + self.prev_error = 0.0 + self.get_logger().info(f'Wall Follow Node started (wall: {self.wall_side})') + + def scan_callback(self, msg: LaserScan): + """ + Process LiDAR scan and maintain distance from wall. + """ + try: + ranges = np.array(msg.ranges) + + # Replace bad values + ranges = np.nan_to_num(ranges, nan=msg.range_max, + posinf=msg.range_max, + neginf=msg.range_max) + + # Get wall distance based on side + if self.wall_side.lower() == 'left': + # Left side scan: indices 0-90 degrees from left + wall_range_indices = range(0, int(len(ranges) * 0.25)) + else: # right + # Right side scan: last 90 degrees + wall_range_indices = range(int(len(ranges) * 0.75), len(ranges)) + + wall_ranges = ranges[wall_range_indices] + wall_distance = np.min(wall_ranges[wall_ranges > 0.1]) + + # Calculate error and derivative + error = self.target_distance - wall_distance + error_derivative = error - self.prev_error + self.prev_error = error + + # PID control + steering = self.kp * error + self.kd * error_derivative + steering = np.clip(steering, -1.5, 1.5) # Limit steering + + # Check for obstacles in front + front_ranges = ranges[int(len(ranges)*0.35):int(len(ranges)*0.65)] + front_obstacle = np.min(front_ranges) + + # Generate command + cmd = Twist() + + if front_obstacle < 0.3: # Obstacle too close ahead + cmd.linear.x = 0.0 + cmd.angular.z = steering * 2.0 # Turn more sharply + else: + cmd.linear.x = self.linear_velocity + cmd.angular.z = steering + + self.velocity_publisher.publish(cmd) + + self.get_logger().debug( + f'Wall dist: {wall_distance:.2f}m, Front: {front_obstacle:.2f}m' + ) + + except Exception as e: + self.get_logger().error(f'Error in scan callback: {str(e)}') + + +def main(args=None): + rclpy.init(args=args) + wall_follow_node = WallFollowNode() + + try: + rclpy.spin(wall_follow_node) + except KeyboardInterrupt: + pass + finally: + wall_follow_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/src/ydlidar_ros2_driver/CMakeLists.txt b/src/ydlidar_ros2_driver/CMakeLists.txt new file mode 100644 index 0000000..af4e2b2 --- /dev/null +++ b/src/ydlidar_ros2_driver/CMakeLists.txt @@ -0,0 +1,88 @@ +# Copyright(c) 2020 eaibot limited. +cmake_minimum_required(VERSION 3.5) +project(ydlidar_ros2_driver C CXX) + +##################ros2############################################# +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +####################find package##################################### +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rmw REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(visualization_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_srvs REQUIRED) + +############## YDLIDAR SDK START##################################### +#find ydlidar_sdk package +find_package(ydlidar_sdk REQUIRED) +############## YDLIDAR SDK END##################################### + +#Include directories +include_directories( + ${PROJECT_SOURCE_DIR} + ${PROJECT_SOURCE_DIR}/src + ${YDLIDAR_SDK_INCLUDE_DIRS}) + +#link library directories +link_directories(${YDLIDAR_SDK_LIBRARY_DIRS}) + +#--------------------------------------------------------------------------------------- +# generate excutable and add libraries +#--------------------------------------------------------------------------------------- +add_executable(${PROJECT_NAME}_node + src/${PROJECT_NAME}_node.cpp) +#--------------------------------------------------------------------------------------- +# link libraries +#-------------------------------------------------------------------------------------- +ament_target_dependencies(${PROJECT_NAME}_node + "rclcpp" + "sensor_msgs" + "visualization_msgs" + "geometry_msgs" + "std_srvs" + ) + +target_link_libraries(${PROJECT_NAME}_node + ${YDLIDAR_SDK_LIBRARIES}) + +#--------------------------------------------------------------------------------------- +# generate excutable and add libraries +#--------------------------------------------------------------------------------------- +add_executable(${PROJECT_NAME}_client + src/${PROJECT_NAME}_client.cpp) +#--------------------------------------------------------------------------------------- +# link libraries +#-------------------------------------------------------------------------------------- +ament_target_dependencies(${PROJECT_NAME}_client + "rclcpp" + "sensor_msgs" + "visualization_msgs" + "geometry_msgs" + "std_srvs" + ) + +#--------------------------------------------------------------------------------------- +# Install +#--------------------------------------------------------------------------------------- +install(TARGETS + ${PROJECT_NAME}_node ${PROJECT_NAME}_client + DESTINATION lib/${PROJECT_NAME}) + +install(DIRECTORY launch params startup config + DESTINATION share/${PROJECT_NAME}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() + diff --git a/src/ydlidar_ros2_driver/LICENSE.txt b/src/ydlidar_ros2_driver/LICENSE.txt new file mode 100644 index 0000000..e69de29 diff --git a/src/ydlidar_ros2_driver/README.md b/src/ydlidar_ros2_driver/README.md new file mode 100644 index 0000000..ab616a4 --- /dev/null +++ b/src/ydlidar_ros2_driver/README.md @@ -0,0 +1,195 @@ +![YDLIDAR](images/YDLidar.jpg "YDLIDAR") +# YDLIDAR ROS2 Driver + +ydlidar_ros2_driver is a new ros package, which is designed to gradually become the standard driver package for ydlidar devices in the ros2 environment. + +## How to [install ROS2](https://index.ros.org/doc/ros2/Installation) +[ubuntu](https://index.ros.org/doc/ros2/Installation/Dashing/Linux-Install-Debians/) + +[windows](https://index.ros.org/doc/ros2/Installation/Dashing/Windows-Install-Binary/) + +## How to Create a ROS2 workspace +[Create a workspace](https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/#create-a-workspace) + + +## Build & Install YDLidar SDK + +ydlidar_ros2_driver depends on YDLidar-SDK library. If you have never installed YDLidar-SDK library or it is out of date, you must first install YDLidar-SDK library. If you have installed the latest version of YDLidar-SDK, skip this step and go to the next step. + +1. Download or clone the [YDLIDAR/YDLidar-SDK](https://github.com/YDLIDAR/YDLidar-SDK) repository on GitHub. +2. Compile and install the YDLidar-SDK under the ***build*** directory following `README.md` of YDLIDAR/YDLidar-SDK. + +## Build ydlidar_ros2_driver + +1. Clone ydlidar_ros2_driver master branch from github for old version: + + `git clone https://github.com/YDLIDAR/ydlidar_ros2_driver.git ydlidar_ros2_ws/src/ydlidar_ros2_driver` + + Clone ydlidar_ros2_driver humble branch from github for humble,jazzy,etc: + + `git clone -b humble https://github.com/YDLIDAR/ydlidar_ros2_driver.git ydlidar_ros2_ws/src/ydlidar_ros2_driver` + +2. Build ydlidar_ros2_driver package : + + ``` + cd ydlidar_ros2_ws + colcon build --symlink-install + ``` + Note: install colcon [see](https://index.ros.org/doc/ros2/Tutorials/Colcon-Tutorial/#install-colcon) + + ![CMAKE Finished](images/finished.png "CMAKE Finished") + + >Note: If the following error occurs, Please install [YDLIDAR/YDLidar-SDK](https://github.com/YDLIDAR/YDLidar-SDK) first. + + ![CMAKE ERROR](images/cmake_error.png "CMAKE ERROR") + +3. Package environment setup : + + `source ./install/setup.bash` + + Note: Add permanent workspace environment variables. + It's convenientif the ROS2 environment variables are automatically added to your bash session every time a new shell is launched: + ``` + echo "source ~/ydlidar_ros2_ws/install/setup.bash" >> ~/.bashrc + source ~/.bashrc + ``` +4. Confirmation + To confirm that your package path has been set, printenv the `grep -i ROS` variable. + ``` + printenv | grep -i ROS + ``` + You should see something similar to: + `OLDPWD=/home/tony/ydlidar_ros2_ws/install` + +5. Create serial port Alias [optional] + ``` + chmod 0777 src/ydlidar_ros2_driver/startup/* + sudo sh src/ydlidar_ros2_driver/startup/initenv.sh + ``` + Note: After completing the previous operation, replug the LiDAR again. + +## Configure LiDAR [Default parameter file](params/ydlidar.yaml) + +``` +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 230400 + lidar_type: 1 + device_type: 0 + isSingleChannel: false + intensity: false + intensity_bit: 0 + sample_rate: 9 + abnormal_check_count: 4 + fixed_resolution: true + reversion: false + inverted: false + auto_reconnect: true + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 64.0 + range_min: 0.01 + frequency: 10.0 + invalid_range_is_inf: false + debug: false +``` +Note: It needs to be modified according to LiDAR actual situation. + +## Run ydlidar_ros2_driver + +##### Run ydlidar_ros2_driver using launch file + +The command format is : + + `ros2 launch ydlidar_ros2_driver [launch file].py` + +1. Connect LiDAR uint(s). + ``` + ros2 launch ydlidar_ros2_driver ydlidar_launch.py + ``` + or + + ``` + launch $(ros2 pkg prefix ydlidar_ros2_driver)/share/ydlidar_ros2_driver/launch/ydlidar.py + ``` +2. RVIZ + ``` + ros2 launch ydlidar_ros2_driver ydlidar_launch_view.py + ``` + ![View](images/view.png "View") + +3. echo scan topic + ``` + ros2 run ydlidar_ros2_driver ydlidar_ros2_driver_client or ros2 topic echo /scan + ``` + +##### Launch file introduction + +The driver offers users a wealth of options when using different launch file. The launch file directory + +is `"ydlidar_ros2_ws/src/ydlidar_ros2_driver/launch"`. All launch files are listed as below : + +| launch file | features | +| ------------------------- | ------------------------------------------------------------ | +| ydlidar.py | Connect to defualt paramters
Publish LaserScan message on `scan` topic | +| ydlidar_launch.py | Connect ydlidar.yaml Lidar specified by configuration parameters
Publish LaserScan message on `scan` topic | +| ydlidar_launch_view.py | Connect ydlidar.yaml Lidar specified by configuration parameters and setup RVIZ
Publish LaserScan message on `scan` topic | + + + +## Publish Topic +| Topic | Type | Description | +|----------------------|-------------------------|--------------------------------------------------| +| `scan` | sensor_msgs/LaserScan | 2D laser scan of the 0-angle ring | + +## Subscribe Service +| Service | Type | Description | +|----------------------|-------------------------|--------------------------------------------------| +| `stop_scan` | std_srvs::Empty | turn off lidar | +| `start_scan` | std_srvs::Empty | turn on lidar | + + + +## Configure ydlidar_ros_driver internal parameter + +The ydlidar_ros2_driver internal parameters are in the launch file, they are listed as below : + +| Parameter name | Data Type | detail | +| -------------- | ------- | ------------------------------------------------------------ | +| port | string | Set Lidar the serial port or IP address
it can be set to `/dev/ttyUSB0`, `192.168.1.11`, etc.
default: `/dev/ydlidar` | +| frame_id | string | Lidar TF coordinate system name.
default: `laser_frame` | +| ignore_array | string | LiDAR filtering angle area
eg: `-90, -80, 30, 40` | +| baudrate | int | Lidar baudrate or network port.
default: `230400` | +| lidar_type | int | Set lidar type
0 -- TYPE_TOF
1 -- TYPE_TRIANGLE
2 -- TYPE_TOF_NET
default: `1` | +| device_type | int | Set device type
0 -- YDLIDAR_TYPE_SERIAL
1 -- YDLIDAR_TYPE_TCP
2 -- YDLIDAR_TYPE_UDP
default: `0` | +| sample_rate | int | Set Lidar Sample Rate.
default: `9` | +| abnormal_check_count | int | Set the number of abnormal startup data attempts.
default: `4` | +| fixed_resolution | bool | Fixed angluar resolution.
default: `true` | +| reversion | bool | Reversion LiDAR.
default: `true` | +| inverted | bool | Inverted LiDAR.
false -- ClockWise.
true -- CounterClockWise
default: `true` | +| auto_reconnect | bool | Automatically reconnect the LiDAR.
true -- hot plug.
default: `true` | +| isSingleChannel | bool | Whether LiDAR is a single-channel.
default: `false` | +| intensity | bool | Whether LiDAR has intensity.
true -- G2 LiDAR.
default: `false` | +| support_motor_dtr | bool | Whether the Lidar can be started and stopped by Serial DTR.
default: `false` | +| angle_min | float | Minimum Valid Angle.
default: `-180` | +| angle_max | float | Maximum Valid Angle.
default: `180` | +| range_min | float | Minimum Valid range.
default: `0.1` | +| range_max | float | Maximum Valid range.
default: `16.0` | +| frequency | float | Set Scanning Frequency.
default: `10.0` | +| invalid_range_is_inf | bool | Invalid Range is inf.
true -- inf.
false -- 0.0.
default: `false` | +More paramters details, see [here](details.md) + +## Contact EAI +![Development Path](images/EAI.png) + +If you have any extra questions, please feel free to [contact us](http://www.ydlidar.cn/cn/contact) + + + + + + diff --git a/src/ydlidar_ros2_driver/config/ydlidar.rviz b/src/ydlidar_ros2_driver/config/ydlidar.rviz new file mode 100644 index 0000000..7c9e799 --- /dev/null +++ b/src/ydlidar_ros2_driver/config/ydlidar.rviz @@ -0,0 +1,177 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /LaserScan1/Topic1 + Splitter Ratio: 0.5 + Tree Height: 617 + - 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 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + 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: 10 + Reference Frame: + Value: true + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/LaserScan + Color: 239; 41; 41 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1012 + Min Color: 0; 0; 0 + Min Intensity: 1008 + Name: LaserScan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05999999865889549 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: System Default + Value: /scan + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/TF + Enabled: true + Frame Timeout: 15 + Frames: + All Enabled: true + base_link: + Value: true + laser_frame: + Value: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: false + Tree: + base_link: + laser_frame: + {} + Update Interval: 0 + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: laser_frame + 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 + 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: 10 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.5103975534439087 + Target Frame: + Value: Orbit (rviz) + Yaw: 6.163581848144531 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002f4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002f4000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002f4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000002f4000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004420000003efc0100000002fb0000000800540069006d00650100000000000004420000000000000000fb0000000800540069006d006501000000000000045000000000000000000000023f000002f400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 67 + Y: 60 diff --git a/src/ydlidar_ros2_driver/details.md b/src/ydlidar_ros2_driver/details.md new file mode 100644 index 0000000..04b1edb --- /dev/null +++ b/src/ydlidar_ros2_driver/details.md @@ -0,0 +1,133 @@ +# ROS Paramters Table + +## Dataset + +
LIDAR Model Baudrate SampleRate(K) Range(m) Frequency(HZ) Intenstiy(bit) SingleChannel voltage(V) +
F4 1 115200 4 0.12~12 5~12 false false 4.8~5.2 +
S4 4 115200 4 0.10~8.0 5~12 (PWM) false false 4.8~5.2 +
S4B 4/11 153600 4 0.10~8.0 5~12(PWM) true(8) false 4.8~5.2 +
S2 4/12 115200 3 0.10~8.0 4~8(PWM) false true 4.8~5.2 +
G4 5 230400 9/8/4 0.28/0.26/0.1~16 5~12 false false 4.8~5.2 +
X4 6 128000 5 0.12~10 5~12(PWM) false false 4.8~5.2 +
X2/X2L 6 115200 3 0.10~8.0 4~8(PWM) false true 4.8~5.2 +
G4PRO 7 230400 9/8/4 0.28/0.26/0.1~16 5~12 false false 4.8~5.2 +
F4PRO 8 230400 4/6 0.12~12 5~12 false false 4.8~5.2 +
R2 9 230400 5 0.12~16 5~12 false false 4.8~5.2 +
G6 13 512000 18/16/8 0.28/0.26/0.1~25 5~12 false false 4.8~5.2 +
G2A 14 230400 5 0.12~12 5~12 false false 4.8~5.2 +
G2 15 230400 5 0.28~16 5~12 true(8) false 4.8~5.2 +
G2C 16 115200 4 0.1~12 5~12 false false 4.8~5.2 +
G4B 17 512000 10 0.12~16 5~12 true(10) false 4.8~5.2 +
G4C 18 115200 4 0.1~12 5~12 false false 4.8~5.2 +
G1 19 230400 9 0.28~16 5~12 false false 4.8~5.2 +
G5 20 230400 9/8/4 0.28/0.26/0.1~16 5~12 false false 4.8~5.2 +
G7 21 512000 18/16/8 0.28/0.26/0.1~25 5~12 false false 4.8~5.2 +
TX8 　 100 115200 4 0.05~8 4~8(PWM) false true 4.8~5.2 +
TX20 　 100 115200 4 0.05~20 4~8(PWM) false true 4.8~5.2 +
TG15 　 100 512000 20/18/10 0.05~30 3~16 false false 4.8~5.2 +
TG30 　 101 512000 20/18/10 0.05~30 3~16 false false 4.8~5.2 +
TG50 　 102 512000 20/18/10 0.05~50 3~16 false false 4.8~5.2 +
T15 　 200 8000 20 0.05~30 5~35 true false 4.8~5.2 +
+ +## Baudrate Table + +| LiDAR | baudrate | +|-----------------------------------------------|-----------------------| +|F4/S2/X2/X2L/S4/TX8/TX20/G4C | 115200 | +|X4 | 128000 | +|S4B | 153600 | +|G1/G2/R2/G4/G5/G4PRO/F4PRO | 230400 | +|G6/G7/TG15/TG30/TG50 | 512000 | +|T5/T15 | 8000 (network port) | + + +## SingleChannel Table + +| LiDAR | isSingleChannel | +|-----------------------------------------------------------|-----------------------| +|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C | false | +|S2/X2/X2L | true | +|TG15/TG30/TG50 | false | +|TX8/TX20 | true | +|T5/T15 | false (optional) | + + +## LidarType Table + +| LiDAR | lidar_type | +|-----------------------------------------------------------------------|-----------------------| +|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L | TYPE_TRIANGLE | +|TG15/TG30/TG50/TX8/TX20 | TYPE_TOF | +|T5/T15 | TYPE_TOF_NET | + +## DeviceType Table + +| LiDAR | lidar_type | +|-----------------------------------------------------------------------|-----------------------| +|G1/G2/G4/G5/G6/G7/F4/F4PRO/S4/S4B/X4/R2/G4C/S2/X2/X2L | YDLIDAR_TYPE_SERIAL | +|TG15/TG30/TG50/TX8/TX20 | YDLIDAR_TYPE_SERIAL | +|T5/T15 | YDLIDAR_TYPE_TCP | + + +## Sampling Rate Table + +| LiDAR | sample_rate | +|-----------------------------|------------------------| +|G4/G5/F4 | 4,8,9 | +|F4PRO | 4,6 | +|G6/G7 | 8,16,18 | +|G2/R2/X4 | 5 | +|G1 | 9 | +|S4/S4B/G4C/TX8/TX20 | 4 | +|S2 | 3 | +|TG15/TG30/TG50 | 10,18,20 | +|T5/T15 | 20 | + + +## Frequency Table + +| LiDAR | frequency | +|-----------------------------------------------|------------------------| +|G1/G2/R2/G6/G7/G4/G5/G4PRO/F4/F4PRO | 5-12Hz | +|S4/S4B/S2/TX8/TX20/X4 | Not Support | +|TG15/TG30/TG50 | 3-16Hz | +|T5/T15 | 5-35Hz | + +Note: For unsupported LiDARs, adjusting the scanning frequency requires external access to PWM speed control. + +## Reversion Table + +
LiDAR reversion +
G1/G2/G2A/G2C/F4/F4PRO/R2 true +
G4/G5/G4PRO/G4B/G4C/G6/G7 true +
TG15/TG30/TG50 true +
T5/T15 true +
S2/X2/X2L/X4/S4/S4B false +
TX8/TX20 false +
+ +## Intensity Table + +
LiDAR intensity +
S4B/G2/G4B true +
G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7 false +
G1/G2A/G2C/R2 false +
S2/X2/X2L/X4 false +
TG15/TG30/TG50 false +
TX8/TX20 false +
T5/T15 true +
false +
+ +## DTR Support Table + + +
LiDAR support_motor_dtr +
S4/S4B/S2/X2/X2L/X4 true +
TX8/TX20 true +
G4/G5/G4C/G4PRO/F4/F4PRO/G6/G7 false +
G1/G2A/G2C/R2/G2/G4B false +
TG15/TG30/TG50 false +
T5/T15 false +
diff --git a/src/ydlidar_ros2_driver/images/EAI.png b/src/ydlidar_ros2_driver/images/EAI.png new file mode 100644 index 0000000..5dee9e2 Binary files /dev/null and b/src/ydlidar_ros2_driver/images/EAI.png differ diff --git a/src/ydlidar_ros2_driver/images/YDLidar.jpg b/src/ydlidar_ros2_driver/images/YDLidar.jpg new file mode 100644 index 0000000..d764165 Binary files /dev/null and b/src/ydlidar_ros2_driver/images/YDLidar.jpg differ diff --git a/src/ydlidar_ros2_driver/images/cmake_error.png b/src/ydlidar_ros2_driver/images/cmake_error.png new file mode 100644 index 0000000..169970a Binary files /dev/null and b/src/ydlidar_ros2_driver/images/cmake_error.png differ diff --git a/src/ydlidar_ros2_driver/images/finished.png b/src/ydlidar_ros2_driver/images/finished.png new file mode 100644 index 0000000..260c306 Binary files /dev/null and b/src/ydlidar_ros2_driver/images/finished.png differ diff --git a/src/ydlidar_ros2_driver/images/rviz.png b/src/ydlidar_ros2_driver/images/rviz.png new file mode 100644 index 0000000..c2a016b Binary files /dev/null and b/src/ydlidar_ros2_driver/images/rviz.png differ diff --git a/src/ydlidar_ros2_driver/images/view.png b/src/ydlidar_ros2_driver/images/view.png new file mode 100644 index 0000000..ad821de Binary files /dev/null and b/src/ydlidar_ros2_driver/images/view.png differ diff --git a/src/ydlidar_ros2_driver/launch/ydlidar.py b/src/ydlidar_ros2_driver/launch/ydlidar.py new file mode 100644 index 0000000..3a80766 --- /dev/null +++ b/src/ydlidar_ros2_driver/launch/ydlidar.py @@ -0,0 +1,29 @@ +from launch.exit_handler import ignore_exit_handler, restart_exit_handler +from ros2run.api import get_executable_path + + +def launch(launch_descriptor, argv): + ld = launch_descriptor + package = 'ydlidar_ros2_driver' + ld.add_process( + cmd=[get_executable_path(package_name=package, executable_name='ydlidar_ros2_driver_node')], + name='ydlidar_ros2_driver_node', + exit_handler=restart_exit_handler, + ) + package = 'tf2_ros' + ld.add_process( + # The XYZ/Quat numbers for base_link -> laser_frame are taken from the + # turtlebot URDF in + # https://github.com/turtlebot/turtlebot/blob/931d045/turtlebot_description/urdf/sensors/astra.urdf.xacro + cmd=[ + get_executable_path( + package_name=package, executable_name='static_transform_publisher'), + '0', '0', '0.02', + '0', '0', '0', '1', + 'base_link', + 'laser_frame' + ], + name='static_tf_pub_laser', + exit_handler=restart_exit_handler, + ) + return ld diff --git a/src/ydlidar_ros2_driver/launch/ydlidar_launch.py b/src/ydlidar_ros2_driver/launch/ydlidar_launch.py new file mode 100644 index 0000000..92d064b --- /dev/null +++ b/src/ydlidar_ros2_driver/launch/ydlidar_launch.py @@ -0,0 +1,56 @@ +#!/usr/bin/python3 +# Copyright 2020, EAIBOT +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('ydlidar_ros2_driver') + parameter_file = LaunchConfiguration('params_file') + node_name = 'ydlidar_ros2_driver_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'ydlidar.yaml'), + description='FPath to the ROS2 parameters file to use.') + + driver_node = LifecycleNode(package='ydlidar_ros2_driver', + node_executable='ydlidar_ros2_driver_node', + node_name='ydlidar_ros2_driver_node', + output='screen', + emulate_tty=True, + parameters=[parameter_file], + node_namespace='/', + ) + tf2_node = Node(package='tf2_ros', + node_executable='static_transform_publisher', + node_name='static_tf_pub_laser', + arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'], + ) + + return LaunchDescription([ + params_declare, + driver_node, + tf2_node, + ]) diff --git a/src/ydlidar_ros2_driver/launch/ydlidar_launch_view.py b/src/ydlidar_ros2_driver/launch/ydlidar_launch_view.py new file mode 100644 index 0000000..a37a99c --- /dev/null +++ b/src/ydlidar_ros2_driver/launch/ydlidar_launch_view.py @@ -0,0 +1,63 @@ +#!/usr/bin/python3 +# Copyright 2020, EAIBOT +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch_ros.actions import LifecycleNode +from launch_ros.actions import Node +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch.actions import LogInfo + +import lifecycle_msgs.msg +import os + + +def generate_launch_description(): + share_dir = get_package_share_directory('ydlidar_ros2_driver') + rviz_config_file = os.path.join(share_dir, 'config','ydlidar.rviz') + parameter_file = LaunchConfiguration('params_file') + node_name = 'ydlidar_ros2_driver_node' + + params_declare = DeclareLaunchArgument('params_file', + default_value=os.path.join( + share_dir, 'params', 'ydlidar.yaml'), + description='FPath to the ROS2 parameters file to use.') + + driver_node = LifecycleNode(package='ydlidar_ros2_driver', + node_executable='ydlidar_ros2_driver_node', + node_name='ydlidar_ros2_driver_node', + output='screen', + emulate_tty=True, + parameters=[parameter_file], + node_namespace='/', + ) + tf2_node = Node(package='tf2_ros', + node_executable='static_transform_publisher', + node_name='static_tf_pub_laser', + arguments=['0', '0', '0.02','0', '0', '0', '1','base_link','laser_frame'], + ) + rviz2_node = Node(package='rviz2', + node_executable='rviz2', + node_name='rviz2', + arguments=['-d', rviz_config_file], + ) + + return LaunchDescription([ + params_declare, + driver_node, + tf2_node, + rviz2_node, + ]) diff --git a/src/ydlidar_ros2_driver/package.xml b/src/ydlidar_ros2_driver/package.xml new file mode 100644 index 0000000..d2f73cc --- /dev/null +++ b/src/ydlidar_ros2_driver/package.xml @@ -0,0 +1,33 @@ + + + + ydlidar_ros2_driver + 1.0.1 + + The ROS2 device driver for YDLIDAR LIDARS + + Tony + MIT + + ament_cmake + + rclcpp + sensor_msgs + visualization_msgs + geometry_msgs + std_srvs + + rclcpp + sensor_msgs + visualization_msgs + geometry_msgs + std_srvs + + ament_cmake_gtest + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/ydlidar_ros2_driver/params/G1.yaml b/src/ydlidar_ros2_driver/params/G1.yaml new file mode 100644 index 0000000..5366f27 --- /dev/null +++ b/src/ydlidar_ros2_driver/params/G1.yaml @@ -0,0 +1,23 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 230400 + lidar_type: 1 + device_type: 0 + sample_rate: 9 + abnormal_check_count: 4 + fixed_resolution: true + reversion: true + inverted: true + auto_reconnect: true + isSingleChannel: false + intensity: false + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 16.0 + range_min: 0.1 + frequency: 10.0 + invalid_range_is_inf: false diff --git a/src/ydlidar_ros2_driver/params/G2.yaml b/src/ydlidar_ros2_driver/params/G2.yaml new file mode 100644 index 0000000..7a28662 --- /dev/null +++ b/src/ydlidar_ros2_driver/params/G2.yaml @@ -0,0 +1,24 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 230400 + lidar_type: 1 + device_type: 0 + sample_rate: 4 + intensity_bit: 10 + abnormal_check_count: 4 + fixed_resolution: true + reversion: true + inverted: true + auto_reconnect: true + isSingleChannel: false + intensity: true + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 16.0 + range_min: 0.1 + frequency: 10.0 + invalid_range_is_inf: false diff --git a/src/ydlidar_ros2_driver/params/G6.yaml b/src/ydlidar_ros2_driver/params/G6.yaml new file mode 100644 index 0000000..da6d3db --- /dev/null +++ b/src/ydlidar_ros2_driver/params/G6.yaml @@ -0,0 +1,23 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 512000 + lidar_type: 1 + device_type: 0 + sample_rate: 18 + abnormal_check_count: 4 + fixed_resolution: true + reversion: true + inverted: true + auto_reconnect: true + isSingleChannel: false + intensity: false + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 25.0 + range_min: 0.1 + frequency: 10.0 + invalid_range_is_inf: false diff --git a/src/ydlidar_ros2_driver/params/GS2.yaml b/src/ydlidar_ros2_driver/params/GS2.yaml new file mode 100644 index 0000000..89c6dcb --- /dev/null +++ b/src/ydlidar_ros2_driver/params/GS2.yaml @@ -0,0 +1,29 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 921600 + lidar_type: 3 + device_type: 0 + isSingleChannel: false + intensity: false + intensity_bit: 0 + m1_mode: 0 + m2_mode: 0 + m3_mode: 1 + sample_rate: 9 + abnormal_check_count: 4 + fixed_resolution: true + reversion: false + inverted: false + auto_reconnect: true + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 1.0 + range_min: 0.025 + frequency: 10.0 + invalid_range_is_inf: false + debug: false + diff --git a/src/ydlidar_ros2_driver/params/TEA.yaml b/src/ydlidar_ros2_driver/params/TEA.yaml new file mode 100644 index 0000000..8758855 --- /dev/null +++ b/src/ydlidar_ros2_driver/params/TEA.yaml @@ -0,0 +1,23 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: 192.168.0.11 + frame_id: laser_frame + ignore_array: "" + baudrate: 8090 + lidar_type: 0 + device_type: 1 + sample_rate: 20 + abnormal_check_count: 4 + fixed_resolution: true + reversion: true + inverted: true + auto_reconnect: true + isSingleChannel: false + intensity: true + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 50.0 + range_min: 0.01 + frequency: 20.0 + invalid_range_is_inf: false diff --git a/src/ydlidar_ros2_driver/params/TG.yaml b/src/ydlidar_ros2_driver/params/TG.yaml new file mode 100644 index 0000000..cf65e8c --- /dev/null +++ b/src/ydlidar_ros2_driver/params/TG.yaml @@ -0,0 +1,23 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 512000 + lidar_type: 0 + device_type: 0 + sample_rate: 20 + abnormal_check_count: 4 + fixed_resolution: true + reversion: true + inverted: true + auto_reconnect: true + isSingleChannel: false + intensity: false + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 50.0 + range_min: 0.01 + frequency: 10.0 + invalid_range_is_inf: false diff --git a/src/ydlidar_ros2_driver/params/TminiPro.yaml b/src/ydlidar_ros2_driver/params/TminiPro.yaml new file mode 100644 index 0000000..e7e70a6 --- /dev/null +++ b/src/ydlidar_ros2_driver/params/TminiPro.yaml @@ -0,0 +1,24 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 230400 + lidar_type: 1 + device_type: 0 + sample_rate: 4 + intensity_bit: 8 + abnormal_check_count: 4 + fixed_resolution: true + reversion: true + inverted: true + auto_reconnect: true + isSingleChannel: false + intensity: true + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 12.0 + range_min: 0.03 + frequency: 10.0 + invalid_range_is_inf: false diff --git a/src/ydlidar_ros2_driver/params/X2.yaml b/src/ydlidar_ros2_driver/params/X2.yaml new file mode 100644 index 0000000..cf9817f --- /dev/null +++ b/src/ydlidar_ros2_driver/params/X2.yaml @@ -0,0 +1,25 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 115200 + lidar_type: 1 + device_type: 0 + sample_rate: 3 + abnormal_check_count: 4 + fixed_resolution: true + reversion: false + inverted: false + auto_reconnect: true + isSingleChannel: true + intensity_bit: 0 + intensity: false + support_motor_dtr: true + angle_max: 180.0 + angle_min: -180.0 + range_max: 12.0 + range_min: 0.1 + frequency: 10.0 + invalid_range_is_inf: false + debug: false diff --git a/src/ydlidar_ros2_driver/params/X4-Pro.yaml b/src/ydlidar_ros2_driver/params/X4-Pro.yaml new file mode 100644 index 0000000..8e49fb9 --- /dev/null +++ b/src/ydlidar_ros2_driver/params/X4-Pro.yaml @@ -0,0 +1,23 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 128000 + lidar_type: 1 + device_type: 0 + sample_rate: 5 + abnormal_check_count: 4 + fixed_resolution: true + reversion: true + inverted: true + auto_reconnect: true + isSingleChannel: true + intensity: false + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 12.0 + range_min: 0.1 + frequency: 10.0 + invalid_range_is_inf: false diff --git a/src/ydlidar_ros2_driver/params/X4.yaml b/src/ydlidar_ros2_driver/params/X4.yaml new file mode 100644 index 0000000..716f2f8 --- /dev/null +++ b/src/ydlidar_ros2_driver/params/X4.yaml @@ -0,0 +1,23 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 128000 + lidar_type: 1 + device_type: 0 + sample_rate: 5 + abnormal_check_count: 4 + fixed_resolution: true + reversion: true + inverted: true + auto_reconnect: true + isSingleChannel: false + intensity: false + support_motor_dtr: true + angle_max: 180.0 + angle_min: -180.0 + range_max: 12.0 + range_min: 0.1 + frequency: 10.0 + invalid_range_is_inf: false diff --git a/src/ydlidar_ros2_driver/params/ydlidar.yaml b/src/ydlidar_ros2_driver/params/ydlidar.yaml new file mode 100644 index 0000000..dce32e7 --- /dev/null +++ b/src/ydlidar_ros2_driver/params/ydlidar.yaml @@ -0,0 +1,25 @@ +ydlidar_ros2_driver_node: + ros__parameters: + port: /dev/ttyUSB0 + frame_id: laser_frame + ignore_array: "" + baudrate: 230400 + lidar_type: 1 + device_type: 0 + isSingleChannel: false + intensity: false + intensity_bit: 0 + sample_rate: 9 + abnormal_check_count: 4 + fixed_resolution: true + reversion: false + inverted: false + auto_reconnect: true + support_motor_dtr: false + angle_max: 180.0 + angle_min: -180.0 + range_max: 64.0 + range_min: 0.01 + frequency: 10.0 + invalid_range_is_inf: false + debug: false diff --git a/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_client.cpp b/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_client.cpp new file mode 100644 index 0000000..7bac433 --- /dev/null +++ b/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_client.cpp @@ -0,0 +1,42 @@ +/* + * YDLIDAR SYSTEM + * YDLIDAR ROS 2 Node Client + * + * Copyright 2017 - 2020 EAI TEAM + * http://www.eaibot.com + * + */ + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include + +#define RAD2DEG(x) ((x)*180./M_PI) + +static void scanCb(sensor_msgs::msg::LaserScan::SharedPtr scan) { + int count = scan->scan_time / scan->time_increment; + printf("[YDLIDAR INFO]: I heard a laser scan %s[%d]:\n", scan->header.frame_id.c_str(), count); + printf("[YDLIDAR INFO]: angle_range : [%f, %f]\n", RAD2DEG(scan->angle_min), + RAD2DEG(scan->angle_max)); + + for (int i = 0; i < count; i++) { + float degree = RAD2DEG(scan->angle_min + scan->angle_increment * i); + printf("[YDLIDAR INFO]: angle-distance : [%f, %f]\n", degree, scan->ranges[i]); + } +} + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_client"); + + auto lidar_info_sub = node->create_subscription( + "scan", rclcpp::SensorDataQoS(), scanCb); + + rclcpp::spin(node); + + rclcpp::shutdown(); + + + return 0; +} diff --git a/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp b/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp new file mode 100644 index 0000000..7ffc708 --- /dev/null +++ b/src/ydlidar_ros2_driver/src/ydlidar_ros2_driver_node.cpp @@ -0,0 +1,263 @@ +/* + * YDLIDAR SYSTEM + * YDLIDAR ROS 2 Node + * + * Copyright 2017 - 2020 EAI TEAM + * http://www.eaibot.com + * + */ + +#ifdef _MSC_VER +#ifndef _USE_MATH_DEFINES +#define _USE_MATH_DEFINES +#endif +#endif + +#include "src/CYdLidar.h" +#include +#include +#include +#include + +#include "rclcpp/clock.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/time_source.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "std_srvs/srv/empty.hpp" +#include +#include +#include +#include + +#define ROS2Verision "1.0.1" + + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + auto node = rclcpp::Node::make_shared("ydlidar_ros2_driver_node"); + + RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Current ROS Driver Version: %s\n", ((std::string)ROS2Verision).c_str()); + + CYdLidar laser; + std::string str_optvalue = "/dev/ydlidar"; + node->declare_parameter("port"); + node->get_parameter("port", str_optvalue); + ///lidar port + laser.setlidaropt(LidarPropSerialPort, str_optvalue.c_str(), str_optvalue.size()); + + ///ignore array + str_optvalue = ""; + node->declare_parameter("ignore_array"); + node->get_parameter("ignore_array", str_optvalue); + laser.setlidaropt(LidarPropIgnoreArray, str_optvalue.c_str(), str_optvalue.size()); + + std::string frame_id = "laser_frame"; + node->declare_parameter("frame_id"); + node->get_parameter("frame_id", frame_id); + + //////////////////////int property///////////////// + /// lidar baudrate + int optval = 230400; + node->declare_parameter("baudrate"); + node->get_parameter("baudrate", optval); + laser.setlidaropt(LidarPropSerialBaudrate, &optval, sizeof(int)); + /// tof lidar + optval = TYPE_TRIANGLE; + node->declare_parameter("lidar_type"); + node->get_parameter("lidar_type", optval); + laser.setlidaropt(LidarPropLidarType, &optval, sizeof(int)); + /// device type + optval = YDLIDAR_TYPE_SERIAL; + node->declare_parameter("device_type"); + node->get_parameter("device_type", optval); + laser.setlidaropt(LidarPropDeviceType, &optval, sizeof(int)); + /// sample rate + optval = 9; + node->declare_parameter("sample_rate"); + node->get_parameter("sample_rate", optval); + laser.setlidaropt(LidarPropSampleRate, &optval, sizeof(int)); + /// abnormal count + optval = 4; + node->declare_parameter("abnormal_check_count"); + node->get_parameter("abnormal_check_count", optval); + laser.setlidaropt(LidarPropAbnormalCheckCount, &optval, sizeof(int)); + + /// Intenstiy bit count + optval = 0; + node->declare_parameter("intensity_bit"); + node->get_parameter("intensity_bit", optval); + laser.setlidaropt(LidarPropIntenstiyBit, &optval, sizeof(int)); + + //////////////////////bool property///////////////// + /// fixed angle resolution + bool b_optvalue = false; + node->declare_parameter("fixed_resolution"); + node->get_parameter("fixed_resolution", b_optvalue); + laser.setlidaropt(LidarPropFixedResolution, &b_optvalue, sizeof(bool)); + /// rotate 180 + b_optvalue = true; + node->declare_parameter("reversion"); + node->get_parameter("reversion", b_optvalue); + laser.setlidaropt(LidarPropReversion, &b_optvalue, sizeof(bool)); + /// Counterclockwise + b_optvalue = true; + node->declare_parameter("inverted"); + node->get_parameter("inverted", b_optvalue); + laser.setlidaropt(LidarPropInverted, &b_optvalue, sizeof(bool)); + b_optvalue = true; + node->declare_parameter("auto_reconnect"); + node->get_parameter("auto_reconnect", b_optvalue); + laser.setlidaropt(LidarPropAutoReconnect, &b_optvalue, sizeof(bool)); + /// one-way communication + b_optvalue = false; + node->declare_parameter("isSingleChannel"); + node->get_parameter("isSingleChannel", b_optvalue); + laser.setlidaropt(LidarPropSingleChannel, &b_optvalue, sizeof(bool)); + /// intensity + b_optvalue = false; + node->declare_parameter("intensity"); + node->get_parameter("intensity", b_optvalue); + laser.setlidaropt(LidarPropIntenstiy, &b_optvalue, sizeof(bool)); + /// Motor DTR + b_optvalue = false; + node->declare_parameter("support_motor_dtr"); + node->get_parameter("support_motor_dtr", b_optvalue); + laser.setlidaropt(LidarPropSupportMotorDtrCtrl, &b_optvalue, sizeof(bool)); + //是否启用调试 + b_optvalue = false; + node->declare_parameter("debug"); + node->get_parameter("debug", b_optvalue); + laser.setEnableDebug(b_optvalue); + + //////////////////////float property///////////////// + /// unit: ° + float f_optvalue = 180.0f; + node->declare_parameter("angle_max"); + node->get_parameter("angle_max", f_optvalue); + laser.setlidaropt(LidarPropMaxAngle, &f_optvalue, sizeof(float)); + f_optvalue = -180.0f; + node->declare_parameter("angle_min"); + node->get_parameter("angle_min", f_optvalue); + laser.setlidaropt(LidarPropMinAngle, &f_optvalue, sizeof(float)); + /// unit: m + f_optvalue = 64.f; + node->declare_parameter("range_max"); + node->get_parameter("range_max", f_optvalue); + laser.setlidaropt(LidarPropMaxRange, &f_optvalue, sizeof(float)); + f_optvalue = 0.1f; + node->declare_parameter("range_min"); + node->get_parameter("range_min", f_optvalue); + laser.setlidaropt(LidarPropMinRange, &f_optvalue, sizeof(float)); + /// unit: Hz + f_optvalue = 10.f; + node->declare_parameter("frequency"); + node->get_parameter("frequency", f_optvalue); + laser.setlidaropt(LidarPropScanFrequency, &f_optvalue, sizeof(float)); + + bool invalid_range_is_inf = false; + node->declare_parameter("invalid_range_is_inf"); + node->get_parameter("invalid_range_is_inf", invalid_range_is_inf); + + //初始化 + bool ret = laser.initialize(); + if (ret) + { + //设置GS工作模式（非GS雷达请无视该代码） + int i_v = 0; + node->declare_parameter("m1_mode"); + node->get_parameter("m1_mode", i_v); + laser.setWorkMode(i_v, 0x01); + i_v = 0; + node->declare_parameter("m2_mode"); + node->get_parameter("m2_mode", i_v); + laser.setWorkMode(i_v, 0x02); + i_v = 1; + node->declare_parameter("m3_mode"); + node->get_parameter("m3_mode", i_v); + laser.setWorkMode(i_v, 0x04); + //启动扫描 + ret = laser.turnOn(); + } + else + { + RCLCPP_ERROR(node->get_logger(), "%s\n", laser.DescribeError()); + } + + auto laser_pub = node->create_publisher("scan", rclcpp::SensorDataQoS()); + + auto stop_scan_service = + [&laser](const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr response) -> bool + { + return laser.turnOff(); + }; + + auto stop_service = node->create_service("stop_scan",stop_scan_service); + + auto start_scan_service = + [&laser](const std::shared_ptr request_header, + const std::shared_ptr req, + std::shared_ptr response) -> bool + { + return laser.turnOn(); + }; + + auto start_service = node->create_service("start_scan",start_scan_service); + + rclcpp::WallRate loop_rate(20); + + //std::ofstream file("pointcloud_data.txt"); // 打开文件流 + while (ret && rclcpp::ok()) + { + LaserScan scan; + if (laser.doProcessSimple(scan)) + { + auto scan_msg = std::make_shared(); + + scan_msg->header.stamp.sec = RCL_NS_TO_S(scan.stamp); + scan_msg->header.stamp.nanosec = scan.stamp - RCL_S_TO_NS(scan_msg->header.stamp.sec); + scan_msg->header.frame_id = frame_id; + scan_msg->angle_min = scan.config.min_angle; + scan_msg->angle_max = scan.config.max_angle; + scan_msg->angle_increment = scan.config.angle_increment; + scan_msg->scan_time = scan.config.scan_time; + scan_msg->time_increment = scan.config.time_increment; + scan_msg->range_min = scan.config.min_range; + scan_msg->range_max = scan.config.max_range; + + int size = (scan.config.max_angle - scan.config.min_angle)/ scan.config.angle_increment + 1; + scan_msg->ranges.resize(size); + scan_msg->intensities.resize(size); + for (size_t i=0; i < scan.points.size(); i++) + { + const auto& p = scan.points.at(i); + int index = std::ceil((scan.points[i].angle - scan.config.min_angle)/scan.config.angle_increment); + if(index >=0 && index < size) { + scan_msg->ranges[index] = scan.points[i].range; + scan_msg->intensities[index] = scan.points[i].intensity; + } + //file << "i:" << i << ",a:" << p.angle << ",d:" << p.range << ",p:" << p.intensity << std::endl; + } + laser_pub->publish(*scan_msg); + } + else + { + RCLCPP_ERROR(node->get_logger(), "Failed to get scan"); + } + if(!rclcpp::ok()) + { + break; + } + rclcpp::spin_some(node); + loop_rate.sleep(); + } + + RCLCPP_INFO(node->get_logger(), "[YDLIDAR INFO] Now YDLIDAR is stopping ......."); + laser.turnOff(); + laser.disconnecting(); + rclcpp::shutdown(); + + return 0; +} diff --git a/src/ydlidar_ros2_driver/startup/initenv.sh b/src/ydlidar_ros2_driver/startup/initenv.sh new file mode 100644 index 0000000..9b5c1d0 --- /dev/null +++ b/src/ydlidar_ros2_driver/startup/initenv.sh @@ -0,0 +1,11 @@ +#!/bin/bash +echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="10c4", ATTRS{idProduct}=="ea60", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar.rules + +echo 'KERNEL=="ttyACM*", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="5740", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-V2.rules + +echo 'KERNEL=="ttyUSB*", ATTRS{idVendor}=="067b", ATTRS{idProduct}=="2303", MODE:="0666", GROUP:="dialout", SYMLINK+="ydlidar"' >/etc/udev/rules.d/ydlidar-2303.rules + +service udev reload +sleep 2 +service udev restart +