diff --git a/mapping/CMakeLists.txt b/mapping/CMakeLists.txt deleted file mode 100644 index fb0f8702..00000000 --- a/mapping/CMakeLists.txt +++ /dev/null @@ -1,86 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(mapping) - -include(../cmake/default_settings.cmake) - -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_components REQUIRED) -find_package(std_msgs REQUIRED) -find_package(nav_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_sensor_msgs REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) -find_package(pcl_conversions REQUIRED) -find_package(PCL 1.10 REQUIRED) - -include_directories( - include - ${PCL_INCLUDE_DIRS} -) -link_directories(${PCL_LIBRARY_DIRS}) -add_definitions(${PCL_DEFINITIONS}) - -# Library creation -add_library(${PROJECT_NAME} SHARED - src/elevation_mapping.cpp -) - -target_link_libraries(${PROJECT_NAME} - ${PCL_LIBRARIES} -) - -set(dependencies - rclcpp - rclcpp_components - std_msgs - sensor_msgs - geometry_msgs - tf2 - tf2_sensor_msgs - tf2_geometry_msgs - nav_msgs - pcl_conversions -) - -ament_target_dependencies(${PROJECT_NAME} - ${dependencies} -) - -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "mapping::ElevationMapping" - EXECUTABLE ${PROJECT_NAME}_ElevationMapping -) - -# Install launch files. -install( - DIRECTORY - config - launch - DESTINATION share/${PROJECT_NAME}/ -) - -# Install library -install(TARGETS - ${PROJECT_NAME} - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/${PROJECT_NAME} -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/mapping/package.xml b/mapping/package.xml deleted file mode 100644 index 82912fb7..00000000 --- a/mapping/package.xml +++ /dev/null @@ -1,30 +0,0 @@ - - - - mapping - 0.0.0 - Convert point cloud from depth camera to elevation map - yambati03 - MIT - - ament_cmake - - ament_lint_auto - ament_lint_common - - rclcpp - rclcpp_components - std_msgs - sensor_msgs - nav_msgs - geometry_msgs - tf2 - tf2_sensor_msgs - tf2_geometry_msgs - pcl_conversions - pcl_1.10 - - - ament_cmake - - \ No newline at end of file diff --git a/urc_bringup/launch/bringup_simulation.launch.py b/urc_bringup/launch/bringup_simulation.launch.py index 49728532..c51fc140 100644 --- a/urc_bringup/launch/bringup_simulation.launch.py +++ b/urc_bringup/launch/bringup_simulation.launch.py @@ -16,7 +16,7 @@ def generate_launch_description(): pkg_gazebo_ros = get_package_share_directory("gazebo_ros") - pkg_urc_gazebo = get_package_share_directory("urc_gazebo") + # pkg_urc_gazebo = get_package_share_directory("urc_gazebo") pkg_urc_bringup = get_package_share_directory("urc_bringup") pkg_path_planning = get_package_share_directory("path_planning") pkg_trajectory_following = get_package_share_directory("trajectory_following") @@ -29,8 +29,7 @@ def generate_launch_description(): use_sim_time = LaunchConfiguration("use_sim_time", default="true") xacro_file = os.path.join( - get_package_share_directory("urc_hw_description"), - "urdf/walli.xacro" + get_package_share_directory("urc_hw_description"), "urdf/walli.xacro" ) assert os.path.exists(xacro_file), "urdf path doesnt exist in " + str(xacro_file) robot_description_config = process_file( @@ -148,13 +147,13 @@ def generate_launch_description(): ) elevation_mapping_node = Node( - package="mapping", - executable="mapping_ElevationMapping", + package="urc_perception", + executable="urc_perception_ElevationMapping", output="screen", parameters=[ PathJoinSubstitution( [ - FindPackageShare("mapping"), + FindPackageShare("urc_perception"), "config", "mapping_params.yaml", ] diff --git a/urc_bringup/launch/bt.launch.py b/urc_bringup/launch/bt.launch.py index ce70c620..a5f354d2 100644 --- a/urc_bringup/launch/bt.launch.py +++ b/urc_bringup/launch/bt.launch.py @@ -28,7 +28,7 @@ def generate_launch_description(): ros_lib_paths = [ os.path.join(node_lib_path_base, lib_name) for lib_name in ros_lib_names ] - bt_file_name = "status_light.xml" + bt_file_name = "bt_test.xml" enable_color = SetEnvironmentVariable(name="RCUTILS_COLORIZED_OUTPUT", value="1") diff --git a/urc_orchestrator/CMakeLists.txt b/urc_orchestrator/CMakeLists.txt index 5adb3aa7..95177ca2 100644 --- a/urc_orchestrator/CMakeLists.txt +++ b/urc_orchestrator/CMakeLists.txt @@ -11,8 +11,6 @@ find_package(urc_msgs REQUIRED) find_package(std_msgs REQUIRED) find_package(nav_msgs REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(OpenCV 4 REQUIRED) find_package(cv_bridge REQUIRED) find_package(image_transport REQUIRED) find_package(tf2 REQUIRED) @@ -35,10 +33,8 @@ set(dependencies urc_msgs std_msgs sensor_msgs - diagnostic_updater cv_bridge image_transport - OpenCV nav_msgs ) @@ -57,7 +53,6 @@ rclcpp_components_register_node( install( DIRECTORY launch - config DESTINATION share/${PROJECT_NAME}/ ) diff --git a/urc_orchestrator/config/fake_config.yaml b/urc_orchestrator/config/fake_config.yaml deleted file mode 100644 index e69de29b..00000000 diff --git a/urc_perception/CMakeLists.txt b/urc_perception/CMakeLists.txt index 4ea8e001..b84a7d19 100644 --- a/urc_perception/CMakeLists.txt +++ b/urc_perception/CMakeLists.txt @@ -3,83 +3,63 @@ project(urc_perception) include(../cmake/default_settings.cmake) -# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_components REQUIRED) -find_package(urc_msgs REQUIRED) find_package(std_msgs REQUIRED) +find_package(nav_msgs REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(diagnostic_updater REQUIRED) -find_package(OpenCV 4 REQUIRED) -find_package(cv_bridge REQUIRED) -find_package(image_transport REQUIRED) +find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) +find_package(tf2_sensor_msgs REQUIRED) find_package(tf2_geometry_msgs REQUIRED) -find_package(nav2_costmap_2d REQUIRED) +find_package(pcl_conversions REQUIRED) +find_package(PCL 1.10 REQUIRED) include_directories( include - ${nav2_costmap_2d_INCLUDE_DIRS} + ${PCL_INCLUDE_DIRS} ) +link_directories(${PCL_LIBRARY_DIRS}) +add_definitions(${PCL_DEFINITIONS}) # Library creation add_library(${PROJECT_NAME} SHARED - src/aruco_detector.cpp - src/aruco_location.cpp - src/depth_laserscan.cpp - src/costmap_generator.cpp + src/elevation_mapping.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${PCL_LIBRARIES} ) set(dependencies rclcpp rclcpp_components - urc_msgs std_msgs sensor_msgs - diagnostic_updater - cv_bridge - image_transport - OpenCV + geometry_msgs tf2 + tf2_sensor_msgs tf2_geometry_msgs - nav2_costmap_2d + nav_msgs + pcl_conversions ) ament_target_dependencies(${PROJECT_NAME} ${dependencies} ) -# Node registration -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "aruco_detector::ArucoDetector" - EXECUTABLE ${PROJECT_NAME}_ArucoDetector -) - -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "aruco_location::ArucoLocation" - EXECUTABLE ${PROJECT_NAME}_ArucoLocation -) - rclcpp_components_register_node( ${PROJECT_NAME} - PLUGIN "depth_laserscan::DepthLaserScan" - EXECUTABLE ${PROJECT_NAME}_DepthLaserScan -) - -rclcpp_components_register_node( - ${PROJECT_NAME} - PLUGIN "costmap_generator::CostmapGenerator" - EXECUTABLE ${PROJECT_NAME}_CostmapGenerator + PLUGIN "urc_perception::ElevationMapping" + EXECUTABLE ${PROJECT_NAME}_ElevationMapping ) # Install launch files. install( DIRECTORY - launch config + launch DESTINATION share/${PROJECT_NAME}/ ) @@ -93,18 +73,14 @@ install(TARGETS if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) - # the following line skips the copyright linker + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files set(ament_cmake_copyright_FOUND TRUE) # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files set(ament_cmake_cpplint_FOUND TRUE) ament_lint_auto_find_test_dependencies() endif() -ament_export_include_directories(msg) - -ament_export_include_directories(include) - -ament_export_libraries(${PROJECT_NAME}) -ament_export_dependencies(${dependencies}) - ament_package() diff --git a/urc_perception/README.md b/urc_perception/README.md deleted file mode 100644 index c0d2895f..00000000 --- a/urc_perception/README.md +++ /dev/null @@ -1,17 +0,0 @@ -# URC Perception - -This package is a collection of nodes focused on detecting and mapping ARUCO tags. - -## Aruco Detector - -Focuses on detecting aruco tags if they are found, and then determining basic data about the found tag. -- Subscribes to an image publisher -- Uses callback method when any image is received: - - Uses image_msg and info_msg - - Uses cv::aruco package methods: detectMarkers and estimatePoseSingleMarkers to determine distance, angles, and marker id. - - Publishes this info continually to the aruco topic. - - -## Aruco Location - -Focuses on using info from Aruco Detector (to which this node publishes) to determine GPS coordinates of the drone. diff --git a/urc_perception/config/aruco_detector_params.yaml b/urc_perception/config/aruco_detector_params.yaml deleted file mode 100644 index 4ba4e061..00000000 --- a/urc_perception/config/aruco_detector_params.yaml +++ /dev/null @@ -1,3 +0,0 @@ -aruco_detector: - ros__parameters: - tagWidth: 20 diff --git a/urc_perception/config/aruco_location_params.yaml b/urc_perception/config/aruco_location_params.yaml deleted file mode 100644 index 8edffca7..00000000 --- a/urc_perception/config/aruco_location_params.yaml +++ /dev/null @@ -1,2 +0,0 @@ -aruco_location: - ros__parameters: diff --git a/urc_perception/config/depth_laserscan_params.yaml b/urc_perception/config/depth_laserscan_params.yaml deleted file mode 100644 index e69de29b..00000000 diff --git a/mapping/config/mapping_params.yaml b/urc_perception/config/mapping_params.yaml similarity index 100% rename from mapping/config/mapping_params.yaml rename to urc_perception/config/mapping_params.yaml diff --git a/urc_perception/include/aruco_detector.hpp b/urc_perception/include/aruco_detector.hpp deleted file mode 100644 index 6ed5aa01..00000000 --- a/urc_perception/include/aruco_detector.hpp +++ /dev/null @@ -1,54 +0,0 @@ -#ifndef ARUCO_DETECTOR_H -#define ARUCO_DETECTOR_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -namespace aruco_detector -{ - -class ArucoDetector : public rclcpp::Node -{ -public: - explicit ArucoDetector(const rclcpp::NodeOptions & options); - -private: - rclcpp::Publisher::SharedPtr aruco_publisher; - image_transport::CameraSubscriber camera_subscriber_center_; - image_transport::CameraSubscriber camera_subscriber_left_; - image_transport::CameraSubscriber camera_subscriber_right_; - - std::vector> corners, rejects; // rejects will likely be unused - std::vector MarkerIDs; - - int tagWidth; //actual tag width in cm - double width; //pixel width - int xCenter, yCenter; - double distance; //Returned distance in m - double xAngle, yAngle; //Should be in radians - - std::vector rvecs, tvecs; - - - const int numTags = 6; //There are 6 valid tags at the URC in 2023 - int detectedTags[6]; //Keeps track of the tags that have already been detected in an image - - void imageCallback( - const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg, - const std::string which_camera - ); -}; -} - -#endif diff --git a/urc_perception/include/aruco_location.hpp b/urc_perception/include/aruco_location.hpp deleted file mode 100644 index faf750ff..00000000 --- a/urc_perception/include/aruco_location.hpp +++ /dev/null @@ -1,59 +0,0 @@ -#ifndef ARUCO_LOCATION_H -#define ARUCO_LOCATION_H - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - - -namespace aruco_location -{ - -class ArucoLocation : public rclcpp::Node -{ -public: - explicit ArucoLocation(const rclcpp::NodeOptions & options); - -private: - double getNextLatitude(double d, double xAngle, double yaw); - double getNextLongitude(double d, double xAngle, double yaw); - double findD(double trueD, double yAngle, double pitch); - - rclcpp::Publisher::SharedPtr location_publisher; - rclcpp::Subscription::SharedPtr aruco_subscriber; - rclcpp::Subscription::SharedPtr gps_subscriber; - rclcpp::Subscription::SharedPtr orientation_subscriber; - - //Variables given by Aruco Detector Node, GPS and IMU sensors. - //gpsRead and orientationRead have the purpose of determining whether data has been received for those yet. - double xAngle, yAngle; - double trueDist; - double tagId; - bool arucoRead = false; - std::string which_camera; - - double droneLat, droneLon, droneAlt; - bool gpsRead = false; - - double yaw = -1, pitch = -1, roll = -1; - bool orientationRead = false; - - void arucoCallback(const urc_msgs::msg::ArucoDetection & aruco_msg); - void gpsCallback(const sensor_msgs::msg::NavSatFix & gps_msg); - void orientationCallback(const sensor_msgs::msg::Imu & imu_msg); -}; - -} - - -#endif diff --git a/urc_perception/include/costmap_generator.hpp b/urc_perception/include/costmap_generator.hpp deleted file mode 100644 index 4236b52c..00000000 --- a/urc_perception/include/costmap_generator.hpp +++ /dev/null @@ -1,41 +0,0 @@ -#ifndef COSTMAP_GENERATOR_H -#define COSTMAP_GENERATOR_H - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace costmap_generator -{ -class CostmapGenerator : public rclcpp::Node -{ -public: - explicit CostmapGenerator(const rclcpp::NodeOptions & options); - -private: - void DepthCallback(const sensor_msgs::msg::Image & msg); - void LaserScanCallback(const sensor_msgs::msg::LaserScan & msg); - void PoseCallback(const geometry_msgs::msg::Pose & msg); - - rclcpp::Subscription::SharedPtr pose_subscriber; - rclcpp::Subscription::SharedPtr laser_subscriber; - rclcpp::Subscription::SharedPtr depth_subscriber; - rclcpp::Publisher::SharedPtr costmap_publisher; - - // Private Variables - nav2_msgs::msg::Costmap costmap; - int direction = 0; - int robotGridX; - int robotGridY; - double minCost; - double maxCost; - int delay; -}; -} - -#endif diff --git a/urc_perception/include/depth_laserscan.hpp b/urc_perception/include/depth_laserscan.hpp deleted file mode 100644 index 56351645..00000000 --- a/urc_perception/include/depth_laserscan.hpp +++ /dev/null @@ -1,22 +0,0 @@ -#include "rclcpp/rclcpp.hpp" -#include "sensor_msgs/msg/image.hpp" -#include "sensor_msgs/msg/laser_scan.hpp" -#include "nav2_costmap_2d/costmap_2d_ros.hpp" -#include "math.h" -#include - - -namespace depth_laserscan -{ -class DepthLaserScan : public rclcpp::Node -{ -public: - explicit DepthLaserScan(const rclcpp::NodeOptions & options); - -private: - void depthCallback(const sensor_msgs::msg::Image::SharedPtr msg); - rclcpp::Subscription::SharedPtr depth_sub_; - rclcpp::Publisher::SharedPtr scan_pub_; - sensor_msgs::msg::LaserScan convertDepthToLaserScan(const sensor_msgs::msg::Image::SharedPtr msg); -}; -} diff --git a/mapping/include/elevation_mapping.hpp b/urc_perception/include/elevation_mapping.hpp similarity index 96% rename from mapping/include/elevation_mapping.hpp rename to urc_perception/include/elevation_mapping.hpp index a5e8ccad..383ac3ee 100644 --- a/mapping/include/elevation_mapping.hpp +++ b/urc_perception/include/elevation_mapping.hpp @@ -9,7 +9,7 @@ #include "tf2_ros/transform_listener.h" #include "tf2_ros/buffer.h" -namespace mapping +namespace urc_perception { class ElevationMapping : public rclcpp::Node @@ -58,6 +58,6 @@ class ElevationMapping : public rclcpp::Node std::string camera_frame_; }; -} // namespace mapping +} // namespace urc_perception #endif // ELEVATION_MAPPING_HPP_ diff --git a/mapping/launch/mapping.launch.py b/urc_perception/launch/mapping.launch.py similarity index 79% rename from mapping/launch/mapping.launch.py rename to urc_perception/launch/mapping.launch.py index d65f77a3..9ea18447 100644 --- a/mapping/launch/mapping.launch.py +++ b/urc_perception/launch/mapping.launch.py @@ -6,13 +6,13 @@ def generate_launch_description(): elevation_mapping_node = Node( - package="mapping", - executable="mapping_ElevationMapping", + package="urc_perception", + executable="urc_perception_ElevationMapping", output="screen", parameters=[ PathJoinSubstitution( [ - FindPackageShare("mapping"), + FindPackageShare("urc_perception"), "config", "mapping_params.yaml", ] diff --git a/urc_perception/launch/perception.launch.py b/urc_perception/launch/perception.launch.py index b5e71829..c8b3c554 100644 --- a/urc_perception/launch/perception.launch.py +++ b/urc_perception/launch/perception.launch.py @@ -1,60 +1,12 @@ from launch import LaunchDescription from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node, SetRemap +from launch_ros.actions import SetRemap from launch_ros.substitutions import FindPackageShare from launch.actions import IncludeLaunchDescription, GroupAction from launch.launch_description_sources import PythonLaunchDescriptionSource def generate_launch_description(): - - aruco_detector_node = Node( - package="urc_perception", - executable="urc_perception_ArucoDetector", - output="screen", - parameters=[ - PathJoinSubstitution( - [ - FindPackageShare("urc_perception"), - "config", - "aruco_detector_params.yaml", - ] - ) - ], - remappings=[("/aruco_detector/aruco_detection", "/aruco_detection")], - ) - - aruco_location_node = Node( - package="urc_perception", - executable="urc_perception_ArucoLocation", - output="screen", - remappings=[("/aruco_location/aruco_location", "/aruco_location")], - ) - - depth_laserscan_node = Node( - package="urc_perception", - executable="urc_perception_DepthLaserScan", - output="screen", - parameters=[ - PathJoinSubstitution( - [ - FindPackageShare("urc_perception"), - "config", - "depth_laserscan_params.yaml", - ] - ) - ], - remappings=[], - ) - - costmap_generator_node = Node( - package="urc_perception", - executable="urc_perception_CostmapGenerator", - output="screen", - parameters=[], - remappings=[], - ) - aruco = IncludeLaunchDescription( PythonLaunchDescriptionSource( [ @@ -69,7 +21,6 @@ def generate_launch_description(): ), ) - # /camera/depth/color/points -> /depth_camera/points realsense = GroupAction( actions=[ SetRemap(src="/camera/depth/color/points", dst="/depth_camera/points"), @@ -89,19 +40,9 @@ def generate_launch_description(): ), ] ) - # realsense = IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # [PathJoinSubstitution([FindPackageShare( - # "realsense2_camera"), "launch", "rs_launch.py"])] - # ), - # ) return LaunchDescription( [ - aruco_detector_node, - aruco_location_node, - depth_laserscan_node, - costmap_generator_node, realsense, aruco, ] diff --git a/urc_perception/package.xml b/urc_perception/package.xml index 6d33debb..288916ce 100644 --- a/urc_perception/package.xml +++ b/urc_perception/package.xml @@ -3,33 +3,26 @@ urc_perception 0.0.0 - Package for detecting the Aruco markers and publishing useful info about them. - a-stickan + Convert point cloud from depth camera to elevation map + yambati03 MIT ament_cmake + ament_lint_auto + ament_lint_common + rclcpp rclcpp_components - urc_msgs - cv_bridge - libopencv-dev - image_transport std_msgs + sensor_msgs nav_msgs + geometry_msgs tf2 + tf2_sensor_msgs tf2_geometry_msgs - sensor_msgs - diagnostic_updater - realsense2_camera - librealsense2 - navigation2 - nav2_costmap_2d - imu_tools - - ament_lint_auto - ament_lint_common - ament_cmake_gtest + pcl_conversions + pcl_1.10 ament_cmake diff --git a/urc_perception/src/aruco_detector.cpp b/urc_perception/src/aruco_detector.cpp deleted file mode 100644 index 1fa01201..00000000 --- a/urc_perception/src/aruco_detector.cpp +++ /dev/null @@ -1,100 +0,0 @@ -#include "aruco_detector.hpp" - -namespace aruco_detector -{ -ArucoDetector::ArucoDetector(const rclcpp::NodeOptions & options) -: rclcpp::Node("aruco_detector", options) -{ - tagWidth = declare_parameter("tagWidth"); - - aruco_publisher = create_publisher( - "~/aruco_detection", - rclcpp::SystemDefaultsQoS()); - - // See --> https://github.com/ros-perception/image_common/issues/121 for imagetransport pub/subs - camera_subscriber_center_ = image_transport::create_camera_subscription( - this, "/image/center_img", - std::bind( - &ArucoDetector::imageCallback, this, std::placeholders::_1, - std::placeholders::_2, "center"), - "raw", rclcpp::SensorDataQoS().get_rmw_qos_profile()); - camera_subscriber_left_ = image_transport::create_camera_subscription( - this, "/image/left_img", - std::bind( - &ArucoDetector::imageCallback, this, std::placeholders::_1, - std::placeholders::_2, "left"), - "raw", rclcpp::SensorDataQoS().get_rmw_qos_profile()); - camera_subscriber_right_ = image_transport::create_camera_subscription( - this, "/image/right_img", - std::bind( - &ArucoDetector::imageCallback, this, std::placeholders::_1, - std::placeholders::_2, "right"), - "raw", rclcpp::SensorDataQoS().get_rmw_qos_profile()); -} - -void ArucoDetector::imageCallback( - const sensor_msgs::msg::Image::ConstSharedPtr & image_msg, - const sensor_msgs::msg::CameraInfo::ConstSharedPtr & info_msg, - const std::string which_camera) -{ - cv::Ptr parameters = cv::aruco::DetectorParameters::create(); - cv::Ptr dictionary = cv::aruco::getPredefinedDictionary( - cv::aruco::DICT_4X4_50); - - // Get image in gray scale - const auto cv_image = cv_bridge::toCvCopy(image_msg, "bgr8"); - cv::cvtColor(cv_image->image, cv_image->image, cv::COLOR_BGR2GRAY); - - const auto camera_matrix = cv::Mat(info_msg->k).reshape(1, 3); // camera intrinsics - cv::Mat distCoeffs = cv::Mat(info_msg->d); - - // Hasn't seen any tags yet - for (int i = 0; i < numTags; ++i) { - detectedTags[i] = 0; - } - - // Converts the image to B&W with 4 different thresholds - for (int i = 0; i < 3; ++i) { - // detects all of the tags with the current b&w threshold - cv::aruco::detectMarkers( - (cv_image->image > i * 60 + 40), dictionary, corners, MarkerIDs, parameters, - rejects); - cv::aruco::estimatePoseSingleMarkers( - corners, tagWidth / 100.0, camera_matrix, distCoeffs, - rvecs, tvecs); - - /* - The below code makes some assumptions: - 1. The only tags that should be published are the tags used at the URC - 2. Only at most one of each tag should be detected - 3. There will be no false positives for tags ids being used at the URC - */ - for (int id = 0; id < static_cast(MarkerIDs.size()); ++id) { - // checks if this tag has already been seen in this image and that it is a valid URC tag - if (MarkerIDs[id] > numTags - 1 || detectedTags[MarkerIDs[id]] == 1) { - continue; - } - detectedTags[MarkerIDs[id]] = 1; - - // Tvecs is a translation vector of the marker relative to the camera in x,y,z - // Rvecs is a rodriguez rotation vector describing the marker orientation relative to the - // marker itself. - xAngle = atan(tvecs[i][0] / tvecs[i][2]); - yAngle = atan(tvecs[i][1] / tvecs[i][2]); - distance = sqrt(pow(tvecs[i][0], 2) + pow(tvecs[i][1], 2) + pow(tvecs[i][2], 2)); - urc_msgs::msg::ArucoDetection aruco_message; - aruco_message.header.stamp = info_msg->header.stamp; - aruco_message.x_angle = xAngle; - aruco_message.y_angle = yAngle; - aruco_message.distance = distance; - aruco_message.id = MarkerIDs[id]; - aruco_message.which_camera = which_camera; - - aruco_publisher->publish(aruco_message); - } - } -} - -} - -RCLCPP_COMPONENTS_REGISTER_NODE(aruco_detector::ArucoDetector) diff --git a/urc_perception/src/aruco_location.cpp b/urc_perception/src/aruco_location.cpp deleted file mode 100644 index f5f5a9c1..00000000 --- a/urc_perception/src/aruco_location.cpp +++ /dev/null @@ -1,130 +0,0 @@ -#include "aruco_location.hpp" - -namespace aruco_location -{ -ArucoLocation::ArucoLocation(const rclcpp::NodeOptions & options) -: rclcpp::Node("aruco_location", options) -{ - location_publisher = create_publisher( - "~/aruco_location", - rclcpp::SystemDefaultsQoS() - ); - - //Aruco Tag Angles and Distance relative to camera. - aruco_subscriber = create_subscription( - "/aruco_detection", rclcpp::SystemDefaultsQoS(), [this](const urc_msgs::msg::ArucoDetection - aruco_msg) { - arucoCallback(aruco_msg); - }); - - gps_subscriber = create_subscription( - "/gps/data", rclcpp::SensorDataQoS(), [this](const sensor_msgs::msg::NavSatFix gps_msg) { - gpsCallback(gps_msg); - }); - - //Yaw, Pitch and Roll (Orientation) - orientation_subscriber = create_subscription( - "/imu/data", rclcpp::SensorDataQoS(), [this](const sensor_msgs::msg::Imu imu_msg) { - orientationCallback(imu_msg); - }); -} - - -/* -All units need to be converted into radians -Latitude and Longtitude calculations using Aviation Formulary V1.47: -See --> http://www.edwilliams.org/avform147.htm#LL -*/ -double ArucoLocation::getNextLatitude(double d, double xAngle, double yaw) -{ - if (!gpsRead || !orientationRead) { - return -1.0; - } - return asin(sin(droneLat) * cos(d) + cos(droneLat) * sin(d) * cos(xAngle + yaw)); - -} - -double ArucoLocation::getNextLongitude(double d, double xAngle, double yaw) -{ - if (!gpsRead || !orientationRead) { - return -1.0; - } - double dlon = - atan2( - sin(xAngle + yaw) * sin(d) * cos(droneLat), - cos(d) - sin(droneLat) * sin(getNextLatitude(d, xAngle, yaw))); - return std::fmod(((droneLon - dlon) + M_PI), 2.0 * M_PI) - M_PI; -} - -double ArucoLocation::findD(double trueD, double yAngle, double pitch) -{ - if (!gpsRead || !orientationRead) { - return -1.0; - } - return trueD * cos(pitch + yAngle); -} - - -/* -This code assumes that the polling rate of the GPS and IMU sensors are high enough -so that when the aruco callback meets its requirements (GPS and Orientation read) the -sensor values will be recent enough to make a Lat/Lon calculation. - -If this becomes a problem, try using callback groups using single-threaded executors -*/ -void ArucoLocation::arucoCallback(const urc_msgs::msg::ArucoDetection & aruco_msg) -{ - //RCLCPP_INFO(this->get_logger(), "Received aruco!"); - xAngle = aruco_msg.x_angle; - yAngle = aruco_msg.y_angle; - trueDist = aruco_msg.distance; - tagId = aruco_msg.id; - which_camera = aruco_msg.which_camera; - - if (gpsRead && orientationRead) { - double d = findD(trueDist, yAngle, pitch); - urc_msgs::msg::ArucoLocation location_message; - location_message.header.stamp = aruco_msg.header.stamp; - location_message.lon = getNextLongitude(d, xAngle, yaw); - location_message.lat = getNextLatitude(d, xAngle, yaw); - location_message.id = aruco_msg.id; - location_message.which_camera = which_camera; - - location_publisher->publish(location_message); - } else { - RCLCPP_INFO( - this->get_logger(), "ARUCO tag GPS and orientation out of date, waiting for their output"); - } - - orientationRead = false, gpsRead = false; -} - -void ArucoLocation::gpsCallback(const sensor_msgs::msg::NavSatFix & gps_msg) -{ - droneLat = static_cast(gps_msg.latitude); - droneLon = static_cast(gps_msg.longitude); - droneAlt = static_cast(gps_msg.altitude); - gpsRead = true; -} - - -/* - See http://wiki.ros.org/tf2/Tutorials/Quaternions and - https://answers.ros.org/question/339528/quaternion-to-rpy-ros2/ -*/ -void ArucoLocation::orientationCallback(const sensor_msgs::msg::Imu & imu_msg) -{ - geometry_msgs::msg::Quaternion rawOrientation = imu_msg.orientation; - tf2::Quaternion convertedOrientation; - tf2::fromMsg(rawOrientation, convertedOrientation); - tf2::Matrix3x3 m(convertedOrientation); - m.getRPY(roll, pitch, yaw); - - roll = static_cast(roll); - pitch = static_cast(pitch); - yaw = static_cast(yaw); - orientationRead = true; -} - -} -RCLCPP_COMPONENTS_REGISTER_NODE(aruco_location::ArucoLocation) diff --git a/urc_perception/src/costmap_generator.cpp b/urc_perception/src/costmap_generator.cpp deleted file mode 100644 index 257e9b6b..00000000 --- a/urc_perception/src/costmap_generator.cpp +++ /dev/null @@ -1,185 +0,0 @@ -#include "costmap_generator.hpp" - -namespace costmap_generator -{ -CostmapGenerator::CostmapGenerator(const rclcpp::NodeOptions & options) -: rclcpp::Node("costmap_generator", options) -{ - depth_subscriber = create_subscription( - "/camera/depth/image_rect_raw", rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::Image msg) {DepthCallback(msg);}); - - pose_subscriber = create_subscription( - "/pose", rclcpp::SystemDefaultsQoS(), - [this](const geometry_msgs::msg::Pose msg) {PoseCallback(msg);}); - laser_subscriber = create_subscription( - "/laserscan", rclcpp::SystemDefaultsQoS(), - [this](const sensor_msgs::msg::LaserScan msg) {LaserScanCallback(msg);}); - - costmap_publisher = create_publisher( - "/costmap", rclcpp::SystemDefaultsQoS()); - - costmap.metadata.resolution = 0.25; // m/cell - costmap.metadata.size_x = 100; - costmap.metadata.size_y = 100; - for (int i = 0; i < costmap.metadata.size_x * costmap.metadata.size_y; ++i) { - costmap.data.push_back(0); - } - RCLCPP_INFO(this->get_logger(), "Costmap Capacity: %d", (int)(costmap.data.size())); - direction = 0; - robotGridX = 0; - robotGridY = 0; - maxCost = 0; - minCost = DBL_MAX; - delay = 0; - - // Needed Feature: - // * Set home position (probably on web interface) that will allow us to put the costmap's left bottom corner where we are - -} - -void CostmapGenerator::PoseCallback(const geometry_msgs::msg::Pose & msg) -{ - double x = msg.orientation.x; - double y = msg.orientation.y; - double z = msg.orientation.z; - double w = msg.orientation.w; - double euler_z = atan2(2.0f * (w * z + x * y), w * w + x * x - y * y - z * z) + M_PI / 4; - int rotation_id = (int(euler_z / (M_PI / 8))) % 8; - RCLCPP_INFO(this->get_logger(), "%d", rotation_id); - direction = rotation_id; -} - - -void CostmapGenerator::DepthCallback(const sensor_msgs::msg::Image & msg) -{ - // 90 91 92 93 94 95 96 97 98 99 - // 80 81 82 83 84 85 86 87 88 89 - // 70 71 72 73 74 75 76 77 78 79 - // 60 61 62 63 64 65 66 67 68 69 - // 50 51 52 53 54 55 56 57 58 59 - // 40 41 42 43 44 45 46 47 48 49 - // 30 31 32 33 34 35 36 37 38 39 - // 20 21 22 23 24 25 26 27 28 29 - // 10 11 12 13 14 15 16 17 18 19 - // 00 01 02 03 04 05 06 07 08 09 - // ^^ robot current position, with each number indicating index in the array - // Directions: - // 0: North - // 1: Northeast - // 2: East - // 3: Southeast - // 4: South - // 5: Southwest - // 6: West - // 7: Northwests - - // Partition the image into 3 partitions, each divided vertically - delay++; - if (delay != 10) { - return; - } - delay = 0; - double costLeft = 0; - double costMiddle = 0; - double costRight = 0; - for (int i = 0; i < msg.width; ++i) { - for (int j = 0; j < msg.height; ++j) { - if ((double)i < msg.width / 3.0) { - costLeft += (255 - msg.data[j * msg.width + i]); - } else if ((double)i < msg.width * 2.0 / 3.0) { - costMiddle += (255 - msg.data[j * msg.width + i]); - } else { - costRight += (255 - msg.data[j * msg.width + i]); - } - } - } - - // Scale relative to highest cost seen - if (costLeft > maxCost) { - maxCost = costLeft; - } - if (costMiddle > maxCost) { - maxCost = costMiddle; - } - if (costRight > maxCost) { - maxCost = costRight; - } - if (costLeft < minCost) { - minCost = costLeft; - } - if (costMiddle < minCost) { - minCost = costMiddle; - } - if (costRight < minCost) { - minCost = costRight; - } - - // Please don't mind this code. - int * data = (int *)malloc(sizeof(int) * 8); - memset(data, 0, sizeof(int) * 8); - - costLeft = (costLeft - minCost) / (maxCost - minCost); - costMiddle = (costMiddle - minCost) / (maxCost - minCost); - costRight = (costRight - minCost) / (maxCost - minCost); - data[(direction - 1) % 8] = costLeft; - data[(direction) % 8] = costMiddle; - data[(direction + 1) % 8] = costRight; - - RCLCPP_INFO(this->get_logger(), "Left Cost: %f", costLeft); - RCLCPP_INFO(this->get_logger(), "Middle Cost: %f", costMiddle); - RCLCPP_INFO(this->get_logger(), "Right Cost: %f", costRight); - - if (robotGridY != costmap.metadata.size_y - 1) { // Direction 0 - costmap.data[(robotGridY + 1) * costmap.metadata.size_x + (robotGridX + 0)] = std::max( - 0, - data[0]); - } - if (robotGridY != costmap.metadata.size_y - 1 && robotGridX != costmap.metadata.size_x - 1) { // Direction 1 - costmap.data[(robotGridY + 1) * costmap.metadata.size_x + (robotGridX + 1)] = std::max( - 0, - data[1]); - } - if (robotGridX != costmap.metadata.size_x - 1) { // Direction 2 - costmap.data[(robotGridY + 0) * costmap.metadata.size_x + (robotGridX + 1)] = std::max( - 0, - data[2]); - } - if (robotGridY != 0 && robotGridX != costmap.metadata.size_x - 1) { // Direction 3 - costmap.data[(robotGridY - 1) * costmap.metadata.size_x + (robotGridX + 1)] = std::max( - 0, - data[3]); - } - if (robotGridY != 0) { // Direction 4 - costmap.data[(robotGridY - 1) * costmap.metadata.size_x + (robotGridX + 0)] = std::max( - 0, - data[4]); - } - if (robotGridY != 0 && robotGridX != 0) { // Direction 5 - costmap.data[(robotGridY - 1) * costmap.metadata.size_x + (robotGridX - 1)] = std::max( - 0, - data[5]); - } - if (robotGridX != 0) { // Direction 6 - costmap.data[(robotGridY + 0) * costmap.metadata.size_x + (robotGridX - 1)] = std::max( - 0, - data[6]); - } - if (robotGridY != costmap.metadata.size_y - 1 && robotGridX != 0) { // Direction 7 - costmap.data[(robotGridY + 1) * costmap.metadata.size_x + (robotGridX - 1)] = std::max( - 0, - data[7]); - } - - - costmap_publisher->publish(costmap); -} -void CostmapGenerator::LaserScanCallback(const sensor_msgs::msg::LaserScan & msg) -{ - // Convert laser scan to costmap - // nav2_msgs::msg::Costmap costmap_msg; - // costmap_publisher->publish(costmap_msg); -} -} - -RCLCPP_COMPONENTS_REGISTER_NODE(costmap_generator::CostmapGenerator) diff --git a/urc_perception/src/depth_laserscan.cpp b/urc_perception/src/depth_laserscan.cpp deleted file mode 100644 index e0f312b6..00000000 --- a/urc_perception/src/depth_laserscan.cpp +++ /dev/null @@ -1,49 +0,0 @@ -#include "depth_laserscan.hpp" - -namespace depth_laserscan -{ -DepthLaserScan::DepthLaserScan(const rclcpp::NodeOptions & options) -: rclcpp::Node("depth_laserscan", options) -{ - depth_sub_ = this->create_subscription( - "/depth_camera/depth/image_raw", 10, - std::bind(&DepthLaserScan::depthCallback, this, std::placeholders::_1)); - scan_pub_ = this->create_publisher("/laserscan", 10); -} - -void DepthLaserScan::depthCallback(const sensor_msgs::msg::Image::SharedPtr msg) -{ - // Convert depth image to laser scan - auto scan_msg = this->convertDepthToLaserScan(msg); - scan_pub_->publish(scan_msg); -} - -sensor_msgs::msg::LaserScan convertDepthToLaserScan(const sensor_msgs::msg::Image::SharedPtr msg) -{ - // Convert depth image to laser scan - sensor_msgs::msg::LaserScan scan_msg; - - // Set basic parameters - scan_msg.header = msg->header; - scan_msg.angle_min = -M_PI / 2; - scan_msg.angle_max = M_PI / 2; - scan_msg.angle_increment = M_PI / 180; - scan_msg.time_increment = 0.0; - scan_msg.range_min = 0.0; - scan_msg.range_max = 10.0; - - auto depth_image = reinterpret_cast(msg->data.data()); - - int num_scan_points = (scan_msg.angle_max - scan_msg.angle_min) / scan_msg.angle_increment; - - scan_msg.ranges.resize(num_scan_points); - - for (int i = 0; i < num_scan_points; i++) { - scan_msg.ranges[i] = depth_image[i]; - } - - return scan_msg; -} -} - -RCLCPP_COMPONENTS_REGISTER_NODE(depth_laserscan::DepthLaserScan) diff --git a/mapping/src/elevation_mapping.cpp b/urc_perception/src/elevation_mapping.cpp similarity index 98% rename from mapping/src/elevation_mapping.cpp rename to urc_perception/src/elevation_mapping.cpp index 4090bffe..2bdbdaa6 100644 --- a/mapping/src/elevation_mapping.cpp +++ b/urc_perception/src/elevation_mapping.cpp @@ -10,7 +10,7 @@ #include #include -namespace mapping +namespace urc_perception { ElevationMapping::ElevationMapping(const rclcpp::NodeOptions & options) @@ -193,7 +193,7 @@ double ElevationMapping::gaussian(double x) return std::exp(-0.5 * x * x / cell_inflation_radius_); } -} // namespace mapping +} // namespace urc_perception #include -RCLCPP_COMPONENTS_REGISTER_NODE(mapping::ElevationMapping) +RCLCPP_COMPONENTS_REGISTER_NODE(urc_perception::ElevationMapping)