Skip to content

Commit

Permalink
param fixed
Browse files Browse the repository at this point in the history
  • Loading branch information
Tim-Desktop committed Sep 22, 2023
1 parent 2917a62 commit 7612e4c
Show file tree
Hide file tree
Showing 4 changed files with 35 additions and 17 deletions.
19 changes: 19 additions & 0 deletions .vscode/c_cpp_properties.json
Original file line number Diff line number Diff line change
@@ -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
}
2 changes: 1 addition & 1 deletion CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -47,4 +47,4 @@ install(DIRECTORY
launch
DESTINATION share/${PROJECT_NAME}/)

ament_package()
ament_package()
2 changes: 1 addition & 1 deletion package.xml
Original file line number Diff line number Diff line change
Expand Up @@ -20,4 +20,4 @@
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
</package>
29 changes: 14 additions & 15 deletions src/potentialF.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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")

{
Expand All @@ -36,7 +36,11 @@ class PotentialField : public rclcpp::Node
// Create Publihser of the Final vector
fin_pub = this->create_publisher<geometry_msgs::msg::PoseStamped>("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);


}

Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -274,25 +281,17 @@ class PotentialField : public rclcpp::Node
// Replusion vector
std::vector<float> V_repulsion ;
//
std::vector<float> 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<PotentialField>(x,y);
auto node = std::make_shared<PotentialField>(argv[1],argv[2]);
rclcpp::spin(node);
// shutdown once finished
rclcpp::shutdown();
Expand Down

0 comments on commit 7612e4c

Please sign in to comment.