From 7612e4c523c93d67652a21a2b4ec5b9b69133955 Mon Sep 17 00:00:00 2001 From: Tim-Desktop Date: Fri, 22 Sep 2023 15:22:55 +0200 Subject: [PATCH] param fixed --- .vscode/c_cpp_properties.json | 19 +++++++++++++++++++ CMakeLists.txt | 2 +- package.xml | 2 +- src/potentialF.cpp | 29 ++++++++++++++--------------- 4 files changed, 35 insertions(+), 17 deletions(-) create mode 100644 .vscode/c_cpp_properties.json diff --git a/.vscode/c_cpp_properties.json b/.vscode/c_cpp_properties.json new file mode 100644 index 0000000..7af989a --- /dev/null +++ b/.vscode/c_cpp_properties.json @@ -0,0 +1,19 @@ +{ + "configurations": [ + { + "name": "Linux", + "includePath": [ + "${workspaceFolder}/**", + "/opt/ros/foxy/include/**", + "/opt/ros/noetic/include/**" + + ], + "defines": [], + "compilerPath": "/usr/bin/clang", + "cStandard": "c17", + "cppStandard": "c++14", + "intelliSenseMode": "linux-clang-x64" + } + ], + "version": 4 +} \ No newline at end of file diff --git a/CMakeLists.txt b/CMakeLists.txt index f7017d9..a0e2e9e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -47,4 +47,4 @@ install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/) -ament_package() +ament_package() \ No newline at end of file diff --git a/package.xml b/package.xml index 5263db5..6641453 100644 --- a/package.xml +++ b/package.xml @@ -20,4 +20,4 @@ ament_cmake - + \ No newline at end of file diff --git a/src/potentialF.cpp b/src/potentialF.cpp index 1d0c294..c6b7f31 100644 --- a/src/potentialF.cpp +++ b/src/potentialF.cpp @@ -18,7 +18,7 @@ using std::placeholders::_1; class PotentialField : public rclcpp::Node { public: - PotentialField(float x_goal, float y_goal) + PotentialField(char* x_goal, char* y_goal) : Node("potential_field_node") { @@ -36,7 +36,11 @@ class PotentialField : public rclcpp::Node // Create Publihser of the Final vector fin_pub = this->create_publisher("Final_Vector",1); - goal = {x_goal, y_goal}; + RCLCPP_INFO(this->get_logger(), "x = %s and y = %s", x_goal,y_goal); + + goal_x = atof(x_goal); + goal_y = atof(y_goal); + } @@ -191,8 +195,11 @@ class PotentialField : public rclcpp::Node //RCLCPP_INFO(this->get_logger(), "Odometry : x = %f | y = %f | theta = %f" , x , y, theta); - ComputeAttraction(goal[0],goal[1]); + ComputeAttraction(goal_x,goal_y); + } + + void scan_callback(sensor_msgs::msg::LaserScan::SharedPtr _msg) { float angle_min = _msg->angle_min; @@ -274,25 +281,17 @@ class PotentialField : public rclcpp::Node // Replusion vector std::vector V_repulsion ; // - std::vector goal; + float goal_x = 0; + float goal_y = 0; }; int main(int argc, char * argv[]) -{ - - printf("You have entered %d arguments:\n", argc); - - for (int i = 0; i < argc; i++) { - printf("%s\n", argv[i]); - } - int x = atoi(argv[0]); - int y = atoi(argv[1]); - +{ // init node rclcpp::init(argc, argv); // init class - auto node = std::make_shared(x,y); + auto node = std::make_shared(argv[1],argv[2]); rclcpp::spin(node); // shutdown once finished rclcpp::shutdown();