From 19533a4e6dcbc2a034a5ff1413302f684ffc4338 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 12:48:07 -0400 Subject: [PATCH 01/18] creating test branch --- catkin_ws/src/f1tenth_modules/node/WallFollowing.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc index aac702e..d934ae8 100644 --- a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc @@ -108,7 +108,7 @@ class WallFollowing // We want this index the angle thats orthogonally // to the left of the front of the car _| bIdx = getScanIdx(M_PI/2.0, lidarData); - aIdx = getScanIdx((M_PI/2.0)-theta, lidarData); + aIdx = getScanIdx((M_PI/2.0)+theta, lidarData); ROS_INFO("Scanning data at angles %f - %f", lidarData.min_angle + (lidarData.scan_inc*aIdx), lidarData.min_angle + (lidarData.scan_inc*bIdx)); From b1ceaba0c2d195ddb704899f760984194f6f2813 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 14:55:14 -0400 Subject: [PATCH 02/18] sim config --- .../src/f1tenth_modules/launch/system.launch | 2 +- .../src/f1tenth_modules/node/WallFollowing.cc | 29 ++++++++++--------- 2 files changed, 17 insertions(+), 14 deletions(-) diff --git a/catkin_ws/src/f1tenth_modules/launch/system.launch b/catkin_ws/src/f1tenth_modules/launch/system.launch index d160e50..4fb6b4c 100644 --- a/catkin_ws/src/f1tenth_modules/launch/system.launch +++ b/catkin_ws/src/f1tenth_modules/launch/system.launch @@ -18,7 +18,7 @@ - + diff --git a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc index 89dd183..22eaaed 100644 --- a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc @@ -72,7 +72,7 @@ class WallFollowing double steer_angle_max = 30.0*M_PI/180.0; bool enabled, done; - bool useSimulator; + bool useSimulator; public: WallFollowing() = delete; @@ -91,7 +91,7 @@ class WallFollowing ROS_ERROR("Could not read from the \"/scan\" topic."); exit(-1); } - + n.getParam("autonomous_drive_topic", driveTopic); n.getParam("wall_follow_idx", muxIdx); @@ -114,7 +114,7 @@ class WallFollowing // subs scanSub = n.subscribe("/scan", 1, &WallFollowing::lidar_cb, this); - + if(useSimulator) { muxSub = n.subscribe("/mux", 1, &WallFollowing::mux_cb, this); @@ -144,16 +144,19 @@ class WallFollowing pose.orientation.w = 1.0; scale.x = scale.y = 0.2; - rvizOpts opts = + if (useSimulator) + { + rvizOpts opts = {.color=0x00ff00, .frame_id="laser_model", .ns="point", .pose=pose, .scale=scale, .topic="/dynamic_viz"}; - rvizPoint = std::make_unique(n, opts); - rvizPoint->addTransformPair("base_link", "laser_model"); + rvizPoint = std::make_unique(n, opts); + rvizPoint->addTransformPair("base_link", "laser_model"); - geometry_msgs::Vector3 v; - v.x = 0.05; - rvizPoint->changeScale(v); + geometry_msgs::Vector3 v; + v.x = 0.05; + rvizPoint->changeScale(v); + } pidHeader.frame_id = "pid_info"; } @@ -179,15 +182,15 @@ class WallFollowing { enabled = true; ROS_INFO("PID enablaed"); - } - else + } + else enabled = false; } void lidar_cb(const sensor_msgs::LaserScan &msg) { /////!!!!! NEED TO FILTER FOR BAD DISTANCES (inf &&&& <0) - + auto a = msg.ranges[aIdx]; auto b = msg.ranges[bIdx]; @@ -250,7 +253,7 @@ class WallFollowing auto steer_angle = (gains.kp*p + gains.ki*i + gains.kd*d); if (steer_angle > steer_angle_max) steer_angle = steer_angle_max; - + const auto steer_ang_deg = steer_angle*(180.0/M_PI); const auto abs_steer_ang_deg = std::abs(steer_ang_deg); From 4d9b039d57aa6e35c1a3b17374b18ea6b2fc28d9 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 15:23:53 -0400 Subject: [PATCH 03/18] consolidating to one launch file --- .../src/f1tenth_modules/launch/system.launch | 16 +++++++++++++++- 1 file changed, 15 insertions(+), 1 deletion(-) diff --git a/catkin_ws/src/f1tenth_modules/launch/system.launch b/catkin_ws/src/f1tenth_modules/launch/system.launch index 4fb6b4c..9573c72 100644 --- a/catkin_ws/src/f1tenth_modules/launch/system.launch +++ b/catkin_ws/src/f1tenth_modules/launch/system.launch @@ -12,12 +12,26 @@ + + + + + + + + + + + + + + + - From 70cf477a99e512390f2557831cc40df7bf86db23 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 15:27:26 -0400 Subject: [PATCH 04/18] removing terminal output --- catkin_ws/src/f1tenth_modules/node/WallFollowing.cc | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc index 22eaaed..5767852 100644 --- a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc @@ -196,7 +196,7 @@ class WallFollowing if (std::isinf(a) || std::isinf(b)) { - ROS_WARN("bad lidar values (inf)"); + // ROS_WARN("bad lidar values (inf)"); return; } @@ -257,7 +257,6 @@ class WallFollowing const auto steer_ang_deg = steer_angle*(180.0/M_PI); const auto abs_steer_ang_deg = std::abs(steer_ang_deg); - ROS_INFO("STEERING ANGLE: %f", steer_angle); // // TODO: Change these limits to compare against radians to minimize conversions // From 1471a19a1acfb78872c09c89ee69411cf31e0441 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 15:31:12 -0400 Subject: [PATCH 05/18] ... --- catkin_ws/src/f1tenth_modules/launch/system.launch | 2 ++ catkin_ws/src/f1tenth_modules/node/WallFollowing.cc | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/catkin_ws/src/f1tenth_modules/launch/system.launch b/catkin_ws/src/f1tenth_modules/launch/system.launch index 9573c72..b672ded 100644 --- a/catkin_ws/src/f1tenth_modules/launch/system.launch +++ b/catkin_ws/src/f1tenth_modules/launch/system.launch @@ -26,6 +26,8 @@ + + diff --git a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc index 5767852..de1494f 100644 --- a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc @@ -202,7 +202,8 @@ class WallFollowing if (a < msg.range_min || b < msg.range_min) { - ROS_WARN("bad lidar values (min range)"); + // ROS_WARN("bad lidar values (min range)"); + return; } geometry_msgs::Point point_a, point_b; From c98dae67352585ff3ca654caf6d7e256c78e6fff Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 15:33:22 -0400 Subject: [PATCH 06/18] ... --- catkin_ws/src/f1tenth_modules/launch/system.launch | 2 -- 1 file changed, 2 deletions(-) diff --git a/catkin_ws/src/f1tenth_modules/launch/system.launch b/catkin_ws/src/f1tenth_modules/launch/system.launch index b672ded..9573c72 100644 --- a/catkin_ws/src/f1tenth_modules/launch/system.launch +++ b/catkin_ws/src/f1tenth_modules/launch/system.launch @@ -26,8 +26,6 @@ - - From c9f33dba2f8c2d147ba022bb26dacaa09edb1155 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 16:24:56 -0400 Subject: [PATCH 07/18] ... --- .../src/f1tenth_modules/include/f1tenth_modules/States.hh | 2 +- catkin_ws/src/f1tenth_modules/node/MuxNode.cc | 4 ++-- catkin_ws/src/f1tenth_modules/node/WallFollowing.cc | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/catkin_ws/src/f1tenth_modules/include/f1tenth_modules/States.hh b/catkin_ws/src/f1tenth_modules/include/f1tenth_modules/States.hh index d14b6f0..abe0c9a 100644 --- a/catkin_ws/src/f1tenth_modules/include/f1tenth_modules/States.hh +++ b/catkin_ws/src/f1tenth_modules/include/f1tenth_modules/States.hh @@ -36,7 +36,7 @@ namespace States { namespace Autonmous { - const std::string NAME = "AUTONMOUS"; + const std::string NAME = "AUTONOMOUS"; const std::string DRIVE_TOPIC = "/auto_drive"; } // namespace Autonmous diff --git a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc index 7cd6480..68f1eca 100644 --- a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc +++ b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc @@ -104,7 +104,7 @@ class Mux // Subcribers muxController = n.subscribe("/input", 1, &Mux::switch_cb, this); - muxIn = n.subscribe("", 1, &Mux::muxIn_cb, this); + muxIn = n.subscribe("", 1, &Mux::muxIn_cb, this); eBrake = n.subscribe("/brake_bool", 1, &Mux::brake_cb, this); // Publishers @@ -118,7 +118,7 @@ class Mux drive.header.frame_id = States::Off::NAME; drive.drive.speed = 0.0; - + auto i = 0; while (i++<50) { diff --git a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc index de1494f..4ee17bf 100644 --- a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc @@ -246,7 +246,7 @@ class WallFollowing // PID Controller p = err; i += err*dt; // may need to be clamped - if (i > steer_angle_max) + if (std::abs(i) > steer_angle_max) i = steer_angle_max; d = (err-prevErr)/dt; From bdb476a789e5043f9bb9ae1737ab9c93b8562b61 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 16:35:43 -0400 Subject: [PATCH 08/18] ... --- catkin_ws/src/f1tenth_modules/node/MuxNode.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc index 68f1eca..a801b4d 100644 --- a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc +++ b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc @@ -33,6 +33,7 @@ class Mux // for every new state added to the system. switch(msg.data) { + ROS_INFO("Current State: %s", currState); case States::Off::INPUT_CHAR: if (currState == States::Off::NAME) break; From becde2b217dad12ee3e03f97989d5db20174667d Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 16:37:55 -0400 Subject: [PATCH 09/18] ... --- catkin_ws/src/f1tenth_modules/node/MuxNode.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc index a801b4d..ca68323 100644 --- a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc +++ b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc @@ -31,9 +31,9 @@ class Mux { // For now, need to manually add a case statement // for every new state added to the system. + ROS_INFO("Current State: %s", currState); switch(msg.data) { - ROS_INFO("Current State: %s", currState); case States::Off::INPUT_CHAR: if (currState == States::Off::NAME) break; From 14999dd1d97f0199ca61c25fca7479c2f29f848d Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 16:39:42 -0400 Subject: [PATCH 10/18] .. --- catkin_ws/src/f1tenth_modules/node/MuxNode.cc | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc index ca68323..3a377e0 100644 --- a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc +++ b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc @@ -31,7 +31,7 @@ class Mux { // For now, need to manually add a case statement // for every new state added to the system. - ROS_INFO("Current State: %s", currState); + ROS_INFO("Current State: %s", currState.c_str()); switch(msg.data) { case States::Off::INPUT_CHAR: From 0d51c3a226411c404c5596e85069b7f16b79adac Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 16:42:34 -0400 Subject: [PATCH 11/18] ... --- catkin_ws/src/f1tenth_modules/node/WallFollowing.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc index 4ee17bf..9f9e327 100644 --- a/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/WallFollowing.cc @@ -196,13 +196,13 @@ class WallFollowing if (std::isinf(a) || std::isinf(b)) { - // ROS_WARN("bad lidar values (inf)"); + ROS_WARN("bad lidar values (inf)"); return; } if (a < msg.range_min || b < msg.range_min) { - // ROS_WARN("bad lidar values (min range)"); + ROS_WARN("bad lidar values (min range)"); return; } From 584b09dd7c999e0b37f1bc4266e7a7c6bad28183 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 16:56:27 -0400 Subject: [PATCH 12/18] trying gf node --- .../src/f1tenth_modules/node/GapFollowing.cc | 54 +++++++++++-------- 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc index 5a6cefc..c5056e4 100644 --- a/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc @@ -39,7 +39,7 @@ class GapFollowing double dispThreshold, dispBufferAngle; int muxIdx; int scanStartIdx, scanEndIdx; - + bool enabled; bool use_simulator; @@ -75,7 +75,7 @@ class GapFollowing muxSub = n.subscribe("/mux", 1, &GapFollowing::mux_cb, this); ROS_INFO("(GAP FOLLOWING): not ussing simulator."); } - else + else { muxSub = n.subscribe("/input", 1, &GapFollowing::key_input, this); ROS_INFO("(GAP FOLLOWING): not using simulator."); @@ -96,7 +96,9 @@ class GapFollowing pose.orientation.w = 1.0; scale.x = scale.y = 0.1; - rvizOpts opts = { + if (use_simulator) + { + rvizOpts opts = { .color=0xff0000, .frame_id="laser", .ns="point", @@ -105,19 +107,20 @@ class GapFollowing .topic="/dynamic_viz" }; - // These points will be green - opts.color=0x00ff00; - cp = std::make_unique(n, opts); - bufferPoints = std::make_unique(n, opts); + // These points will be green + opts.color=0x00ff00; + cp = std::make_unique(n, opts); + bufferPoints = std::make_unique(n, opts); - opts.color=0xff0000; - fp = std::make_unique(n, opts); - bubble = std::make_unique(n, opts); + opts.color=0xff0000; + fp = std::make_unique(n, opts); + bubble = std::make_unique(n, opts); - cp->addTransformPair("base_link", "laser"); - fp->addTransformPair("base_link", "laser"); - bubble->addTransformPair("base_link", "laser"); - bufferPoints->addTransformPair("base_link", "laser"); + cp->addTransformPair("base_link", "laser"); + fp->addTransformPair("base_link", "laser"); + bubble->addTransformPair("base_link", "laser"); + bufferPoints->addTransformPair("base_link", "laser"); + } } void mux_cb(const std_msgs::Int32MultiArray &msg) @@ -172,10 +175,13 @@ class GapFollowing }; // Rviz - closestPoint.p.x = closestPoint.dist*std::cos(closestPoint.angle); - closestPoint.p.y = closestPoint.dist*std::sin(closestPoint.angle); - closestPoint.p.z = 0.0; - cp->addTranslation(closestPoint.p); + if (use_simulator) + { + closestPoint.p.x = closestPoint.dist*std::cos(closestPoint.angle); + closestPoint.p.y = closestPoint.dist*std::sin(closestPoint.angle); + closestPoint.p.z = 0.0; + cp->addTranslation(closestPoint.p); + } // calculate start and end range of the bubble within the scan if (closestPoint.dist < rb) @@ -298,7 +304,8 @@ class GapFollowing } } - bufferPoints->addTranslation(bp); + if (use_simulator) + bufferPoints->addTranslation(bp); ////////////// // Find the largest point away from us within the max sequence @@ -324,9 +331,12 @@ class GapFollowing }; // Rviz - furthestPoint.p.x = furthestPoint.dist*std::cos(furthestPoint.angle); - furthestPoint.p.y = furthestPoint.dist*std::sin(furthestPoint.angle); - fp->addTranslation(furthestPoint.p); + if (use_simulator) + { + furthestPoint.p.x = furthestPoint.dist*std::cos(furthestPoint.angle); + furthestPoint.p.y = furthestPoint.dist*std::sin(furthestPoint.angle); + fp->addTranslation(furthestPoint.p); + } // Set the steering angle to the farthest point // (TODO) Set this up to be a helper function with custom structs From c1f1d621bc838c2215a9e362f062ba439c0df5eb Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 17:02:45 -0400 Subject: [PATCH 13/18] ... --- catkin_ws/src/f1tenth_modules/node/GapFollowing.cc | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc index c5056e4..bc76966 100644 --- a/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc @@ -59,7 +59,7 @@ class GapFollowing exit(-1); n.getParam("gap_follow_idx", muxIdx); - n.getParam("gap_follow_topic", driveTopic); + n.getParam("autonomous_drive_topic", driveTopic); n.getParam("rb", rb); n.getParam("disparity_threshold", dispThreshold); n.getParam("disparity_buffer_angle", dispBufferAngle); @@ -131,7 +131,10 @@ class GapFollowing void key_input(const std_msgs::UInt8 &msg) { if (msg.data == States::GapFollowing::INPUT_CHAR) + { + ROS_INFO("Gap Following enabled"); enabled = true; + } else enabled = false; } From 02f9e5834f729646a2c36133ca42afa20cd0fe20 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 17:08:00 -0400 Subject: [PATCH 14/18] ... --- catkin_ws/src/f1tenth_modules/node/GapFollowing.cc | 3 +++ 1 file changed, 3 insertions(+) diff --git a/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc index bc76966..252750a 100644 --- a/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc @@ -350,7 +350,10 @@ class GapFollowing drive.drive.speed = 1.5; if (enabled) + { + ROS_INFO("Publishing drive message"); drivePub.publish(drive); + } } }; From bb29a7421feb9f0f0efd8ea149ea98a10d062dc1 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Sat, 15 Oct 2022 17:11:45 -0400 Subject: [PATCH 15/18] ... --- catkin_ws/src/f1tenth_modules/node/GapFollowing.cc | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc index 252750a..7a3e2ad 100644 --- a/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc +++ b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc @@ -84,10 +84,10 @@ class GapFollowing scanStartIdx = getScanIdx((-M_PI/3.0), lidarData); scanEndIdx = getScanIdx((M_PI/3.0), lidarData); - ROS_INFO(""); - ROS_INFO("Start index of scan : %d", scanStartIdx); - ROS_INFO("End index of scan: %d", scanEndIdx); - ROS_INFO(""); + // ROS_INFO(""); + // ROS_INFO("Start index of scan : %d", scanStartIdx); + // ROS_INFO("End index of scan: %d", scanEndIdx); + // ROS_INFO(""); // Rviz configuration geometry_msgs::Pose pose; From e9b832f8cb6cb4cd7d06c5986c561e1bd9dcc066 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Wed, 19 Oct 2022 09:54:55 -0400 Subject: [PATCH 16/18] testing launch files --- .../launch/tests/mux_test.launch | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) create mode 100644 catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch diff --git a/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch b/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch new file mode 100644 index 0000000..97e5933 --- /dev/null +++ b/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 8fff5319d152a3b9a3a9d258354a2013b692ca1b Mon Sep 17 00:00:00 2001 From: NMMallick Date: Wed, 19 Oct 2022 20:43:54 -0400 Subject: [PATCH 17/18] configuring joystick stuff --- .../f1tenth_modules/launch/tests/mux_test.launch | 16 +++++++++++++++- catkin_ws/src/f1tenth_modules/node/JoyStick.cc | 4 ++-- catkin_ws/src/f1tenth_modules/node/MuxNode.cc | 15 +++++++++++++-- 3 files changed, 30 insertions(+), 5 deletions(-) diff --git a/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch b/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch index 97e5933..718f683 100644 --- a/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch +++ b/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch @@ -1,5 +1,7 @@ + + @@ -14,8 +16,20 @@ + + + + + + + + + + - + + + diff --git a/catkin_ws/src/f1tenth_modules/node/JoyStick.cc b/catkin_ws/src/f1tenth_modules/node/JoyStick.cc index a406f29..d9e4c7c 100644 --- a/catkin_ws/src/f1tenth_modules/node/JoyStick.cc +++ b/catkin_ws/src/f1tenth_modules/node/JoyStick.cc @@ -16,7 +16,7 @@ class JoyStick ackermann_msgs::AckermannDriveStamped drive; std::string id {"joy_node"}; - std::string pubTopic {"/manual"}; + std::string pubTopic {"/manual_drive"}; std::string subTopic {"/joy"}; float maxSpeed; @@ -48,7 +48,7 @@ class JoyStick // Speed auto x = msg.axes[1]; // Steering - auto y = msg.axes[3]; + auto y = msg.axes[2]; drive.header.frame_id = id; drive.header.stamp = ros::Time::now(); diff --git a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc index 3a377e0..12bed88 100644 --- a/catkin_ws/src/f1tenth_modules/node/MuxNode.cc +++ b/catkin_ws/src/f1tenth_modules/node/MuxNode.cc @@ -19,7 +19,8 @@ class Mux ros::Subscriber muxController; ros::Subscriber eBrake; - std::string currState; + std::string currState, masterDriveTopic; + bool useSimulator; // main drive topic publisher void muxIn_cb(const ackermann_msgs::AckermannDriveStamped &msg) @@ -108,8 +109,18 @@ class Mux muxIn = n.subscribe("", 1, &Mux::muxIn_cb, this); eBrake = n.subscribe("/brake_bool", 1, &Mux::brake_cb, this); + n.param("useSimulator", useSimulator, false); + + if (useSimulator) + { + masterDriveTopic = "/drive"; + ROS_INFO("MUX: Using simulator drive topic /drive"); + } + else + masterDriveTopic = "vesc_cmd"; + // Publishers - muxOut = n.advertise("/vesc_cmd", 1); + muxOut = n.advertise(masterDriveTopic, 1); } void brake() From a30ce6f5aace1bd5eb539ef397844bdc00687d97 Mon Sep 17 00:00:00 2001 From: NMMallick Date: Thu, 20 Oct 2022 09:40:59 -0400 Subject: [PATCH 18/18] made waypoint logging script --- catkin_ws/src/f1tenth_modules/CMakeLists.txt | 4 +- .../launch/tests/mux_test.launch | 18 ++-- .../src/f1tenth_modules/msg/JoyButtons.msg | 4 + .../src/f1tenth_modules/node/JoyStick.cc | 26 ++++++ .../scripts/waypoint_logger.py | 82 +++++++++++++++++++ 5 files changed, 127 insertions(+), 7 deletions(-) create mode 100644 catkin_ws/src/f1tenth_modules/msg/JoyButtons.msg create mode 100755 catkin_ws/src/f1tenth_modules/scripts/waypoint_logger.py diff --git a/catkin_ws/src/f1tenth_modules/CMakeLists.txt b/catkin_ws/src/f1tenth_modules/CMakeLists.txt index 61f55c4..49bbfa7 100644 --- a/catkin_ws/src/f1tenth_modules/CMakeLists.txt +++ b/catkin_ws/src/f1tenth_modules/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs + geometry_msgs message_generation roslaunch tf2 @@ -59,6 +60,7 @@ add_message_files( FILES PointDist.msg PidInfo.msg + JoyButtons.msg ) ## Generate services in the 'srv' folder @@ -113,7 +115,7 @@ generate_messages( catkin_package( INCLUDE_DIRS include LIBRARIES f1tenth_modules - CATKIN_DEPENDS roscpp sensor_msgs std_msgs message_runtime + CATKIN_DEPENDS roscpp sensor_msgs std_msgs geometry_msgs message_runtime # DEPENDS system_lib ) diff --git a/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch b/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch index 718f683..a791f47 100644 --- a/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch +++ b/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch @@ -1,18 +1,22 @@ + + + + - + - + - + @@ -32,17 +36,19 @@ - - - + + + + + diff --git a/catkin_ws/src/f1tenth_modules/msg/JoyButtons.msg b/catkin_ws/src/f1tenth_modules/msg/JoyButtons.msg new file mode 100644 index 0000000..15715f2 --- /dev/null +++ b/catkin_ws/src/f1tenth_modules/msg/JoyButtons.msg @@ -0,0 +1,4 @@ +bool a +bool b +bool x +bool y \ No newline at end of file diff --git a/catkin_ws/src/f1tenth_modules/node/JoyStick.cc b/catkin_ws/src/f1tenth_modules/node/JoyStick.cc index d9e4c7c..0186c0b 100644 --- a/catkin_ws/src/f1tenth_modules/node/JoyStick.cc +++ b/catkin_ws/src/f1tenth_modules/node/JoyStick.cc @@ -1,6 +1,7 @@ #include #include #include +#include /** * @brief Class to subscribe and convert joystick commands @@ -13,21 +14,26 @@ class JoyStick ros::NodeHandle n; ros::Subscriber joySub; ros::Publisher joyPub; + ros::Publisher buttonPub; ackermann_msgs::AckermannDriveStamped drive; std::string id {"joy_node"}; std::string pubTopic {"/manual_drive"}; + std::string buttonTopic {"/joy_buttons"}; std::string subTopic {"/joy"}; float maxSpeed; float maxSteeringAngle; + f1tenth_modules::JoyButtons buttons; + public: JoyStick() : n(ros::NodeHandle("~")) { // Pub-Sub initialization joyPub = n.advertise(pubTopic, 1); + buttonPub = n.advertise(buttonTopic, 1); joySub = n.subscribe(subTopic, 1, &JoyStick::joy_callback, this); // Creating an initial, full brake drive message @@ -41,10 +47,30 @@ class JoyStick // TODO: rosparam these maxSpeed = 7.0; maxSteeringAngle = 0.4189; + + // Set button states + buttons.a = false; + buttons.b = false; + buttons.x = false; + buttons.y = false; + + buttonPub.publish(buttons); } void joy_callback(const sensor_msgs::Joy &msg) { + // Publish any changes that we get from the buttons + if (msg.buttons[0] != buttons.x || msg.buttons[1] != buttons.a + || msg.buttons[2] != buttons.b || msg.buttons[3] != buttons.y) + { + buttons.x = static_cast(msg.buttons[0]); + buttons.a = static_cast(msg.buttons[1]); + buttons.b = static_cast(msg.buttons[2]); + buttons.y = static_cast(msg.buttons[3]); + + buttonPub.publish(buttons); + } + // Speed auto x = msg.axes[1]; // Steering diff --git a/catkin_ws/src/f1tenth_modules/scripts/waypoint_logger.py b/catkin_ws/src/f1tenth_modules/scripts/waypoint_logger.py new file mode 100755 index 0000000..c8a06bc --- /dev/null +++ b/catkin_ws/src/f1tenth_modules/scripts/waypoint_logger.py @@ -0,0 +1,82 @@ +#!/usr/bin/env python3 +import rospy +import os +import math +import sys +from geometry_msgs.msg import PoseStamped +from f1tenth_modules.msg import JoyButtons +from time import gmtime, strftime + +class WaypointLogger: + def __init__(self): + self.waypoints = [] + self.threshold = 0.05 + self.enabled = False + + rospy.Subscriber('/gt_pose', PoseStamped, self.pose_cb) + rospy.Subscriber('/joy_buttons', JoyButtons, self.button_cb) + + def writeFile(self): + print(os.getcwd()) + dir = os.getcwd() + file = open(strftime(dir + '/log/latest/wp-%Y-%m-%d-%H-%M-%S', gmtime())+'.csv', 'w') + for point in self.waypoints: + file.write('%f, %f\n' %(point[0], point[1])) + file.close() + + def filterPoints(self): + ctr = 0 + for i, p in enumerate(self.waypoints): + if i != len(self.waypoints)-1: + for j, q in enumerate(self.waypoints[i+1:]): + dist = math.sqrt(math.pow(q[0] - p[0], 2) + math.pow(q[1] - p[1], 2)) + if dist < self.threshold: + self.waypoints.pop(j) + ctr+=1 + + rospy.logdebug(f"(WAYPOINT LOGGER) Filtered out {ctr} points") + + + def pose_cb(self, msg): + + if self.enabled: + x = msg.pose.position.x + y = msg.pose.position.y + + if len(self.waypoints) == 0: + self.waypoints.append((x, y)) + return + + dist = math.sqrt(math.pow(x - self.waypoints[-1][0], 2) + + math.pow(y - self.waypoints[-1][1], 2)) + if dist > self.threshold: + self.waypoints.append((x, y)) + + def button_cb(self, msg): + ## Toggle enabled + if msg.a: + self.enabled = not self.enabled + if self.enabled: + rospy.loginfo("(WAYPOINT LOGGER) recording waypoints") + else: + rospy.loginfo("(WAYPOINT LOGGER) recording pause") + + if msg.b and not self.enabled: + rospy.loginfo("(WAYPOINT LOGGER) writing waypoints to disk") + self.filterPoints() + self.writeFile() + self.waypoints.clear() + + if msg.y: + rospy.logdebug(f"(WAYPOINT LOGGER) length of list {len(self.waypoints)}") + +def main(args): + logger = WaypointLogger() + rospy.init_node('waypoint_logger', anonymous=True, log_level=rospy.DEBUG) + try: + rospy.spin() + except rospy.ROSInterruptException: + pass + +if __name__ == '__main__': + main(sys.argv) \ No newline at end of file