diff --git a/src/robot/costmap copy/CMakeLists.txt b/src/robot/costmap copy/CMakeLists.txt new file mode 100644 index 0000000..6453e18 --- /dev/null +++ b/src/robot/costmap copy/CMakeLists.txt @@ -0,0 +1,47 @@ +cmake_minimum_required(VERSION 3.10) +project(costmap) + +# Set compiler to use C++ 17 standard +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# Search for dependencies required for building this package +find_package(ament_cmake REQUIRED) # ROS2 build tool +find_package(rclcpp REQUIRED) # ROS2 C++ package +find_package(sensor_msgs REQUIRED) +find_package(nav_msgs REQUIRED) + +# Compiles source files into a library +# A library is not executed, instead other executables can link +# against it to access defined methods and classes. +# We build a library so that the methods defined can be used by +# both the unit test and ROS2 node executables. +add_library(costmap_lib + src/costmap_core.cpp) +# Indicate to compiler where to search for header files +target_include_directories(costmap_lib + PUBLIC include) +# Add ROS2 dependencies required by package +ament_target_dependencies(costmap_lib rclcpp sensor_msgs nav_msgs) + +# Create ROS2 node executable from source files +add_executable(costmap_node src/costmap_node.cpp) +# Link to the previously built library to access costmap classes and methods +target_link_libraries(costmap_node costmap_lib) + +# Copy executable to installation location +install(TARGETS + costmap_node + DESTINATION lib/${PROJECT_NAME}) + +# Copy launch and config files to installation location +install(DIRECTORY + config + DESTINATION share/${PROJECT_NAME}) + +ament_package() \ No newline at end of file diff --git a/src/robot/costmap copy/config/params.yaml b/src/robot/costmap copy/config/params.yaml new file mode 100644 index 0000000..544d49f --- /dev/null +++ b/src/robot/costmap copy/config/params.yaml @@ -0,0 +1,3 @@ +# costmap_node: +# ros__parameters: + diff --git a/src/robot/costmap copy/include/costmap_core.hpp b/src/robot/costmap copy/include/costmap_core.hpp new file mode 100644 index 0000000..b8baa59 --- /dev/null +++ b/src/robot/costmap copy/include/costmap_core.hpp @@ -0,0 +1,21 @@ +#ifndef COSTMAP_CORE_HPP_ +#define COSTMAP_CORE_HPP_ + +#include "rclcpp/rclcpp.hpp" + +namespace robot +{ + +class CostmapCore { + public: + // Constructor, we pass in the node's RCLCPP logger to enable logging to terminal + explicit CostmapCore(const rclcpp::Logger& logger); + + private: + rclcpp::Logger logger_; + +}; + +} + +#endif \ No newline at end of file diff --git a/src/robot/costmap copy/include/costmap_node.hpp b/src/robot/costmap copy/include/costmap_node.hpp new file mode 100644 index 0000000..4ba745d --- /dev/null +++ b/src/robot/costmap copy/include/costmap_node.hpp @@ -0,0 +1,39 @@ +#ifndef COSTMAP_NODE_HPP_ +#define COSTMAP_NODE_HPP_ + +#include "rclcpp/rclcpp.hpp" +#include "sensor_msgs/msg/laser_scan.hpp" +#include "costmap_core.hpp" +#include "nav_msgs/msg/occupancy_grid.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "std_msgs/msg/string.hpp" + +class CostmapNode : public rclcpp::Node { + public: + CostmapNode(); + + // Place callback function here + void publishMessage(int (*arr)[300]); + void publishCostmap(int (*arr)[300]); + void lidar_sub(const sensor_msgs::msg::LaserScan::SharedPtr scan); + void odom_sub(const nav_msgs::msg::Odometry::SharedPtr odom); + + private: + robot::CostmapCore costmap_; + // Place these constructs here + rclcpp::Publisher::SharedPtr string_pub_; + rclcpp::Publisher::SharedPtr grid_pub_; + //rclcpp::TimerBase::SharedPtr timer_; + //rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Subscription::SharedPtr lidar_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + float x_ = -1; + float y_ = -1; + float dir_x_ = -100; + float dir_ = 0; + float dir_y_ = -100; + int x_grid_ = 0; + int y_grid_ = 0; +}; + +#endif diff --git a/src/robot/costmap copy/package.xml b/src/robot/costmap copy/package.xml new file mode 100644 index 0000000..a05d730 --- /dev/null +++ b/src/robot/costmap copy/package.xml @@ -0,0 +1,28 @@ + + + costmap + 0.0.0 + A sample ROS package for pubsub communication + + Owen Leather + Apache2.0 + + + ament_cmake + rclcpp + + + std_msgs + sensor_msgs + nav_msgs + string + + ament_lint_auto + ament_lint_common + ament_cmake_gtest + + + + ament_cmake + + \ No newline at end of file diff --git a/src/robot/costmap copy/src/costmap_core.cpp b/src/robot/costmap copy/src/costmap_core.cpp new file mode 100644 index 0000000..2fd0c92 --- /dev/null +++ b/src/robot/costmap copy/src/costmap_core.cpp @@ -0,0 +1,8 @@ +#include "costmap_core.hpp" + +namespace robot +{ + +CostmapCore::CostmapCore(const rclcpp::Logger& logger) : logger_(logger) {} + +} \ No newline at end of file diff --git a/src/robot/costmap copy/src/costmap_node.cpp b/src/robot/costmap copy/src/costmap_node.cpp new file mode 100644 index 0000000..31e0fc2 --- /dev/null +++ b/src/robot/costmap copy/src/costmap_node.cpp @@ -0,0 +1,189 @@ +#include +#include +#include + +#include "costmap_node.hpp" +const int GRIDSIZE = 300; + +CostmapNode::CostmapNode() : Node("costmap"), costmap_(robot::CostmapCore(this->get_logger())) { + // Initialize the constructs and their parameters + string_pub_ = this->create_publisher("/test_topic", 50); + lidar_sub_ = this->create_subscription( + "/lidar", 10, std::bind(&CostmapNode::lidar_sub, this, std::placeholders::_1)); + odom_sub_ = this->create_subscription( + "/odom/filtered", 10, std::bind(&CostmapNode::odom_sub, this, std::placeholders::_1)); + grid_pub_ = this->create_publisher("/costmap", 10); + //timer_ = this->create_wall_timer(std::chrono::seconds(1), std::bind(&CostmapNode::publishMessage, this)); +} + +/* +// Define the timer to publish a message every 500ms +void CostmapNode::publishMessage(float x) { + auto message = std_msgs::msg::String(); + message.data = std::to_string(x); + RCLCPP_INFO(this->get_logger(), "Coord: '%s'", message.data.c_str()); + string_pub_->publish(message); +}*/ + +// Function to calculate inflated costs +void inflateObstacles(int grid[300][300], double inflationRadius, int maxCost) { + int inflationCells = static_cast(std::round(inflationRadius * 10)); + for (int x = 0; x < GRIDSIZE; x++) { + for (int y = 0; y < GRIDSIZE; y++) { + if (grid[x][y] == 100) { + + for (int dx = -inflationCells; dx <= inflationCells; dx++) { + for (int dy = -inflationCells; dy <= inflationCells; dy++) { + int nx = x + dx; + int ny = y + dy; + + if (nx >= 0 && nx < GRIDSIZE && ny >= 0 && ny < GRIDSIZE) { + double distance = std::sqrt(dx * dx + dy * dy) / 10; + + if (distance > inflationRadius) { + continue; + } + + int cost = static_cast(maxCost * (1.0 - distance / inflationRadius)); + + grid[nx][ny] = std::max(grid[nx][ny], cost); + } + } + } + } + } + } +} + +void CostmapNode::publishMessage(int (*grid)[300]) { + std::stringstream ss; + ss << "coordinates" << static_cast(std::round(this->x_)) << " " << static_cast(std::round( this->y_))<< " "; + ss << "\n"; + ss << "array coordinates?" <x_grid_ << " " << this->y_grid_ << " "; + ss << "\n"; + ss << "direction" <dir_y_, this->dir_x_) << "|| "; + ss << "\n"; + for (int y = 0; y < GRIDSIZE - 1; y+=5) { + for (int x = 0; x < GRIDSIZE -1 ; x+=5) { + if(grid[x][y] > 50){ + ss << "X" << " "; + } + else{ + ss << ". "; + } + + } + ss << "\n"; + } + auto message = std_msgs::msg::String(); + message.data = ss.str(); + RCLCPP_INFO(this->get_logger(), "Costmap:\n%s", message.data.c_str()); + string_pub_->publish(message); +} + +void CostmapNode::publishCostmap(int (*arr)[300]){ + auto msg = nav_msgs::msg::OccupancyGrid(); + msg.header.stamp = this->now(); + msg.header.frame_id = "map"; + msg.data.resize(300*300 + 1); + for (int x = 0; x < 300; ++x){ + for(int y = 0; y < 300; ++y){ + msg.data[x*300 + y] = arr[x][y]; + } + } + grid_pub_->publish(msg); +} + +void CostmapNode::odom_sub(const nav_msgs::msg::Odometry::SharedPtr odom){ + float float_x = odom->pose.pose.position.x; + float float_y = odom->pose.pose.position.y; + + double x = odom->pose.pose.orientation.x; + double y = odom->pose.pose.orientation.y; + double z = odom->pose.pose.orientation.z; + double w = odom->pose.pose.orientation.w; + + double yaw = std::atan2(2.0 * (w * z + x * y), 1.0 - 2.0 * (y * y + z * z)); + + this->x_ = 10*float_x; + this->y_ = 10*float_y; + this->dir_x_ = std::cos(yaw); + this->dir_y_ = std::sin(yaw); + this->dir_ = std::atan2(this->dir_y_, this->dir_x_); + //change direction the robot is facing + /* + auto message = std_msgs::msg::String(); + message.data = "X: " + std::to_string(x) + ", Y: " + std::to_string(y); + // Log and publish the message + RCLCPP_INFO(this->get_logger(), "Coordinates: '%s'", message.data.c_str()); + string_pub_->publish(message);*/ +} + +void CostmapNode::lidar_sub(const sensor_msgs::msg::LaserScan::SharedPtr scan){ + auto message = std_msgs::msg::String(); + if(this->x_ == -1 && this->dir_x_ == -100 && this->y_ == -1){ + return; + } + float angle = std::atan2(this->dir_y_, this->dir_x_); + //for(auto i: scan->ranges){ + //publishMessage(i); + //} + int array[300][300] = {0}; + + // Step 1: Initialize costmap + //initializeCostmap(); + + // Step 2: Convert LaserScan to grid and mark obstacles + for (size_t i = 0; i < scan->ranges.size(); ++i) { + double laser_angle = angle + scan->angle_min + i * scan->angle_increment; + double range = scan->ranges[i]; + if (range < scan->range_max && range > scan->range_min) { + // Calculate grid coordinates + int x_grid = static_cast(range*std::cos(laser_angle)*10 + this->x_) + 150;// add 150 to make bottom corner 0,0 + int y_grid = static_cast(range*std::sin(laser_angle)*10 + this->y_) + 150;// add 150 to make bottom corner 0,0 + this->x_grid_ = x_grid; + this->y_grid_ = y_grid; + if(x_grid < 300 && x_grid >= 0 && y_grid < 300 && y_grid >= 0 ){ + array[x_grid][y_grid] = 100; + } + } + } + + // Step 3: Inflate obstacles + inflateObstacles(array, 1.6, 100); + //publishMessage(array); + // Step 4: Publish costmap + auto msg = nav_msgs::msg::OccupancyGrid(); + msg.header = scan->header; + msg.info.width = 300; + msg.info.height = 300; + msg.info.resolution = 0.1; + msg.info.origin.position.x = -15; // X coordinate in meters + msg.info.origin.position.y = -15; // Y coordinate in meters +// global_map_.info.origin.position.z = 0.0; // Z coordinate (usually 0 for 2D maps) + + // Set the orientation as a quaternion (identity orientation) + msg.info.origin.orientation.x = 0.0; // Quaternion x + msg.info.origin.orientation.y = 0.0; // Quaternion y + msg.info.origin.orientation.z = 0.0; // Quaternion z + msg.info.origin.orientation.w = 1.0; + + msg.data.resize(300*300); + for (int x = 0; x < 300; ++x){ + for(int y = 0; y < 300; ++y){ + msg.data[y*300 + x] = array[x][y]; + } + } + grid_pub_->publish(msg); + //publishCostmap(array); +} + + + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +} diff --git a/watod b/watod index 21bc326..b473a24 100755 --- a/watod +++ b/watod @@ -96,11 +96,11 @@ elif [ -f "$MONO_DIR/watod-config.sh" ]; then fi MODE_OF_OPERATION=${MODE_OF_OPERATION:-"deploy"} -PLATFORM=${PLATFORM:-"amd64"} +PLATFORM=${PLATFORM:-"arm64"} # generate .env file from watod_scripts/watod-setup-docker-env.sh if [ ! -z $VERBOSE ]; then # if we want to see all verbose coming from setting up env - cd $MONO_DIR && . ./watod_scripts/watod-setup-docker-env.sh + cd $MONO_DIR && . ./wato_f1tenth/watod_scripts/watod-setup-docker-env.sh else cd $MONO_DIR && . ./watod_scripts/watod-setup-docker-env.sh &> /dev/null fi diff --git a/watod-config.sh b/watod-config.sh index 9cc462a..c48ebfb 100644 --- a/watod-config.sh +++ b/watod-config.sh @@ -23,7 +23,7 @@ ACTIVE_MODULES="vis_tools robot sim" ## - deploy (default) : runs production-grade containers (non-editable) ## - develop : runs developer containers (editable) -MODE_OF_OPERATION="develop" +#MODE_OF_OPERATION="develop" ############################## ADVANCED CONFIGURATIONS ############################## ## Name to append to docker containers. DEFAULT = "" diff --git a/watod_scripts/watod-setup-docker-env.sh b/watod_scripts/watod-setup-docker-env.sh old mode 100644 new mode 100755