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/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/launch/system.launch b/catkin_ws/src/f1tenth_modules/launch/system.launch index d160e50..9573c72 100644 --- a/catkin_ws/src/f1tenth_modules/launch/system.launch +++ b/catkin_ws/src/f1tenth_modules/launch/system.launch @@ -12,13 +12,27 @@ + + + + + + + + + + + + + + + - - + 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..a791f47 --- /dev/null +++ b/catkin_ws/src/f1tenth_modules/launch/tests/mux_test.launch @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 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/GapFollowing.cc b/catkin_ws/src/f1tenth_modules/node/GapFollowing.cc index 5a6cefc..7a3e2ad 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; @@ -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); @@ -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."); @@ -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; @@ -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) @@ -128,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; } @@ -172,10 +178,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 +307,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 +334,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 @@ -337,7 +350,10 @@ class GapFollowing drive.drive.speed = 1.5; if (enabled) + { + ROS_INFO("Publishing drive message"); drivePub.publish(drive); + } } }; diff --git a/catkin_ws/src/f1tenth_modules/node/JoyStick.cc b/catkin_ws/src/f1tenth_modules/node/JoyStick.cc index a406f29..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"}; + 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,14 +47,34 @@ 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 - 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 7cd6480..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) @@ -31,6 +32,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.c_str()); switch(msg.data) { case States::Off::INPUT_CHAR: @@ -104,11 +106,21 @@ 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); + 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() @@ -118,7 +130,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 93f4278..9f9e327 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); @@ -129,8 +129,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)); @@ -145,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"; } @@ -180,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]; @@ -201,6 +203,7 @@ class WallFollowing if (a < msg.range_min || b < msg.range_min) { ROS_WARN("bad lidar values (min range)"); + return; } geometry_msgs::Point point_a, point_b; @@ -243,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; @@ -251,11 +254,10 @@ 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); - ROS_INFO("STEERING ANGLE: %f", steer_angle); // // TODO: Change these limits to compare against radians to minimize conversions // 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