From 6a7387a6c71191282acc8147816f8adcf22c4f7c Mon Sep 17 00:00:00 2001 From: jlyon805 Date: Wed, 20 Nov 2019 14:47:21 -0800 Subject: [PATCH 1/3] Changed code to be platform-independent --- .../src/cpp/socketsender/main.cpp | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/src/fsae_electric_vehicle/src/cpp/socketsender/main.cpp b/src/fsae_electric_vehicle/src/cpp/socketsender/main.cpp index 90759c2b..6834bb15 100644 --- a/src/fsae_electric_vehicle/src/cpp/socketsender/main.cpp +++ b/src/fsae_electric_vehicle/src/cpp/socketsender/main.cpp @@ -4,11 +4,23 @@ #include "fsae_electric_vehicle/speedometer.h" #include "std_msgs/String.h" -#include -#include -#pragma comment(lib, "Ws2_32.lib") +#ifdef _WIN32 + #include + #include + #pragma comment(lib, "Ws2_32.lib") +#else + #include + #include + #include + #include + #include +#endif -SOCKET sd; +#ifdef _WIN32 + SOCKET sd; +#else + int sd; +#endif void speedCallback(const fsae_electric_vehicle::speedometer::ConstPtr& msg) { ROS_INFO("I heard: [%f]", msg->speed); @@ -18,7 +30,7 @@ void speedCallback(const fsae_electric_vehicle::speedometer::ConstPtr& msg) { struct sockaddr_in addr; std::memset(&addr, 0, sizeof(addr)); addr.sin_family = AF_INET; - InetPton(AF_INET, "127.0.0.1", &(addr.sin_addr)); + inet_pton(AF_INET, "127.0.0.1", &(addr.sin_addr)); //changed to ANSI version for compatibility addr.sin_port = htons(1234); sendto(sd, buf, 4, 0, reinterpret_cast(&addr), sizeof(addr)); } @@ -35,8 +47,14 @@ int main(int argc, char **argv) { addr.sin_port = htons(1232); int optval = 1; + + #ifdef _WIN32 setsockopt(sd, SOL_SOCKET, SO_EXCLUSIVEADDRUSE, reinterpret_cast(&optval), sizeof optval); + #else + setsockopt(sd, SOL_SOCKET, SO_REUSEADDR, + reinterpret_cast(&optval), sizeof optval); + #endif bind(sd, reinterpret_cast(&addr), sizeof(addr)); @@ -49,6 +67,11 @@ int main(int argc, char **argv) { ros::spin(); - ::shutdown(sd, SD_BOTH); - closesocket(sd); + #ifdef _WIN32 + shutdown(sd, SD_BOTH); + closesocket(sd); + #else + shutdown(sd, SHUT_RDWR); + close(sd); + #endif } From 84cd6da2b2ddd8b297d8211c460a0d6726fea830 Mon Sep 17 00:00:00 2001 From: jlyon805 Date: Sun, 8 Dec 2019 23:39:16 -0800 Subject: [PATCH 2/3] Basic GPS and Steering publisher. --- .../src/cpp/gps/main.cpp | 24 ++++++++++- .../src/cpp/steering/main.cpp | 43 +++++++++++++++++++ 2 files changed, 65 insertions(+), 2 deletions(-) create mode 100644 src/fsae_electric_vehicle/src/cpp/steering/main.cpp diff --git a/src/fsae_electric_vehicle/src/cpp/gps/main.cpp b/src/fsae_electric_vehicle/src/cpp/gps/main.cpp index fe5aa391..14665a08 100644 --- a/src/fsae_electric_vehicle/src/cpp/gps/main.cpp +++ b/src/fsae_electric_vehicle/src/cpp/gps/main.cpp @@ -1,3 +1,6 @@ +#include +#include + #include "ros/ros.h" #include "fsae_electric_vehicle/gps.h" @@ -17,11 +20,28 @@ int main(int argc, char **argv) { ros::Rate loop_rate{5}; std::cout << "Init!" << std::endl; - int x = 0; + // Coordinates for JD Hall + float x = 34.241993; + float y = -118.528624; + + float alt = 0; + float sats = 0; + float t_start = time(0); // current time + + srand(static_cast (time(0))); while (ros::ok()) { gps.latitude = x; - x++; + gps.longitude = y; + gps.alt = alt; + gps.sats = sats; + gps.time = time(0) - t_start; + + x += (-0.00001) + static_cast(rand()) / + (static_cast (RAND_MAX/(0.00001+0.00001))); // set values for simulation ONLY + y += (-0.00001) + static_cast(rand()) / + (static_cast (RAND_MAX/(0.00001+0.00001))); + gps_topic.publish(gps); ros::spinOnce(); loop_rate.sleep(); diff --git a/src/fsae_electric_vehicle/src/cpp/steering/main.cpp b/src/fsae_electric_vehicle/src/cpp/steering/main.cpp new file mode 100644 index 00000000..976a9d66 --- /dev/null +++ b/src/fsae_electric_vehicle/src/cpp/steering/main.cpp @@ -0,0 +1,43 @@ +#include + +#include "ros/ros.h" +#include "fsae_electric_vehicle/steering.h" + +#define PI 3.1415926535 + +int main(int argc, char **argv) { + std::cout << "WHAT!?" << std::endl; + ros::init(argc, argv, "Steering"); + std::cout << "After init" << std::endl; + ros::NodeHandle n; + std::cout << "After node handle" << std::endl; + + ros::Publisher steering_topic = n.advertise + ("steering", 1000); + + std::cout << "After publisher" << std::endl; + + fsae_electric_vehicle::steering steering; + + ros::Rate loop_rate{5}; + std::cout << "Init!" << std::endl; + + //assumes data output includes values from 0-1023 + //int x = 0.0; + int pos = 1023/2; //middle forward position + + while (ros::ok()) { + steering.value = pos; + + // check if this produces sinusoidal output + //x += 0.01; + //pos = static_cast(1023/2 * sin(x) + 1023/2); + + (if pos < 1023) + pos++; + + steering_topic.publish(steering); + ros::spinOnce(); + loop_rate.sleep(); + } +} From 40acec10a59749240fd8997c2398bf00d256bc43 Mon Sep 17 00:00:00 2001 From: jlyon805 Date: Sun, 8 Dec 2019 23:48:29 -0800 Subject: [PATCH 3/3] Added linear output between 0-1023 for steering simulation. --- src/fsae_electric_vehicle/src/cpp/steering/main.cpp | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/fsae_electric_vehicle/src/cpp/steering/main.cpp b/src/fsae_electric_vehicle/src/cpp/steering/main.cpp index 976a9d66..856a3df1 100644 --- a/src/fsae_electric_vehicle/src/cpp/steering/main.cpp +++ b/src/fsae_electric_vehicle/src/cpp/steering/main.cpp @@ -25,6 +25,8 @@ int main(int argc, char **argv) { //assumes data output includes values from 0-1023 //int x = 0.0; int pos = 1023/2; //middle forward position + bool right == true; + while (ros::ok()) { steering.value = pos; @@ -33,8 +35,16 @@ int main(int argc, char **argv) { //x += 0.01; //pos = static_cast(1023/2 * sin(x) + 1023/2); - (if pos < 1023) + //should produce pure linear output (like a triangle wave) + if(right == true) pos++; + else if(right == false) + pos--; + + if(pos == 1023) + right = false; + else if(pos == 0) + right = true; steering_topic.publish(steering); ros::spinOnce();