Merge pull request #13 from simchanu29/develop

Merge Develop into Master

ArchyROS.xml

@@ -0,0 +1 @@
+<mxfile userAgent="Mozilla/5.0 (X11; Linux x86_64) AppleWebKit/537.36 (KHTML, like Gecko) draw.io/7.9.5 Chrome/58.0.3029.110 Electron/1.7.5 Safari/537.36" version="8.7.7" editor="www.draw.io" type="device"><diagram id="82940445-f222-5b34-2bde-918a0ddb3653" name="Page-1">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</diagram></mxfile>

helios_control/helios_command/CMakeLists.txt

@@ -134,10 +134,14 @@ include_directories(
 ## Declare a C++ executable
 ## With catkin_make all packages are built within a single CMake context
 ## The recommended prefix ensures that target names across packages don't collide
-add_executable(line_pub src/line_pub.cpp)
-add_executable(path_pub src/path_pub.cpp)
-add_executable(regulation src/regulation.cpp)
-#add_executable(navigation src/navigation.cpp)
+add_executable(line_pub src/doodles/line_pub.cpp)
+add_executable(path_pub src/doodles/path_pub.cpp)
+add_executable(regulation
+        src/regulation_heading.cpp
+        src/algoreg/lineFollow.cpp
+        src/algoreg/algo.cpp)
+add_executable(missionConverter src/mission_convert.cpp)
+add_executable(PIDHandler src/PIDHandler.cpp)
 
 ## Rename C++ executable without prefix
 ## The above recommended prefix causes long target names, the following renames the
@@ -150,13 +154,15 @@ add_executable(regulation src/regulation.cpp)
 add_dependencies(line_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(path_pub ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 add_dependencies(regulation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-#add_dependencies(navigation ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(missionConverter ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(PIDHandler ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
 
 ## Specify libraries to link a library or executable target against
 target_link_libraries(line_pub ${catkin_LIBRARIES})
 target_link_libraries(path_pub ${catkin_LIBRARIES})
 target_link_libraries(regulation ${catkin_LIBRARIES} proj)
-#target_link_libraries(navigation ${catkin_LIBRARIES} proj)
+target_link_libraries(missionConverter ${catkin_LIBRARIES} proj)
+target_link_libraries(PIDHandler ${catkin_LIBRARIES} proj)
 
 #############
 ## Install ##

helios_control/helios_command/include/geodesy/utility.h

@@ -5,6 +5,7 @@
 
 #include <cmath>
 #include <proj_api.h>
+#include <iostream>
 
 #define CONVERSION_FACTOR_GPS 1852.0 // in meters/min
 #define RAD_2_DEG 1.0/M_PI*180.0
@@ -18,10 +19,6 @@ static double angle_rad(double a1,double a2){
     return (fmod( a1 + a2 + 3*M_PI, 2*M_PI ) - M_PI);
 }
 
-//static double enu2ned_yaw_rad(){
-//
-//}
-
 static double ned2enu_yaw_rad(double yaw){
     return angle_rad(M_PI/2,- yaw);
 }
@@ -55,13 +52,16 @@ static double distance(double x1, double y1, double x2, double y2){
     return sqrt( pow(x2-x1,2) + pow(y2-y1,2) );
 }
 
+//static double distance(std::vector<double> wp1, std::vector<double> wp2){
+//    return sqrt( pow(wp2[0]-wp1[0],2) + pow(wp2[1]-wp1[1],2) );
+//}
 
 struct Coordinates{
     double x;
     double y;
 };
 
-Coordinates latlon2meters(double lat, double lon){
+static Coordinates latlon2meters(double lat, double lon){
 
     Coordinates coord;
     projPJ pj_lambert, pj_latlong, pj_utm30N;

helios_control/helios_command/src/PIDHandler.cpp

@@ -0,0 +1,112 @@
+//
+// Created by simon on 25/06/18.
+// Licensed to Simon CHANU
+//
+
+#include <ros/ros.h>
+#include <geometry_msgs/Twist.h>
+#include <geometry_msgs/Pose.h>
+#include <tf/transform_datatypes.h>
+#include <std_msgs/Float64.h>
+
+class PIDHandler {
+    /**
+     * Gère les 2 PID pour la commande linéaire et la commande angulaire
+     */
+public:
+    PIDHandler(){
+        // Sub
+        sub_cmd = node.subscribe("cmd_vel", 1, &PIDHandler::updateCmdVel, this);
+        sub_pose = node.subscribe("pose_est", 1, &PIDHandler::updateHeading, this);
+        sub_twist = node.subscribe("twist_est", 1, &PIDHandler::updateVel, this);
+        sub_pid_vel = node.subscribe("pid_vel/control_effort", 1, &PIDHandler::updatePIDVel, this);
+        sub_pid_head = node.subscribe("pid_head/control_effort", 1, &PIDHandler::updatePIDHead, this);
+
+        // Pub
+        pub_pidStateHead = node.advertise<std_msgs::Float64>("pid_head/state", 1);
+        pub_pidCmdHead = node.advertise<std_msgs::Float64>("pid_head/setpoint", 1);
+        pub_pidStateSpeed = node.advertise<std_msgs::Float64>("pid_vel/state", 1);
+        pub_pidCmdSpeed = node.advertise<std_msgs::Float64>("pid_vel/setpoint", 1);
+        pub_cmd_thrust = node.advertise<geometry_msgs::Twist>("cmd_thrust", 1);
+
+        cmd_thrust.linear.x = 0.0;
+        cmd_thrust.angular.z = 0.0;
+        heading.data = 0.0;
+    }
+
+    void updateHeading(const geometry_msgs::Pose::ConstPtr& msg){
+        heading.data = tf::getYaw(msg->orientation);
+        pub_pidStateHead.publish(heading);
+    }
+
+    void updateVel(const geometry_msgs::Twist::ConstPtr& msg){
+        std_msgs::Float64 vel;
+        vel.data = msg->linear.x;
+        pub_pidStateSpeed.publish(vel);
+    }
+
+    void updateCmdVel(const geometry_msgs::Twist::ConstPtr& msg){
+        std_msgs::Float64 cmdHead;
+        std_msgs::Float64 cmdVel;
+        cmdHead.data = msg->angular.z + heading.data; // On se replace dans un repere global
+        cmdVel.data = msg->linear.x;
+        pub_pidCmdHead.publish(cmdHead);
+        pub_pidCmdSpeed.publish(cmdVel);
+    }
+
+    void updatePIDVel(const std_msgs::Float64::ConstPtr& msg){
+        cmd_thrust.linear.x = msg->data;
+    }
+
+    void updatePIDHead(const std_msgs::Float64::ConstPtr& msg){
+        cmd_thrust.angular.z = msg->data;
+    }
+
+    void spin(){
+        ros::Rate loop(10);
+        while (ros::ok()){
+            // call all waiting callbacks
+            ros::spinOnce();
+
+            // publish the command
+            pub_cmd_thrust.publish(cmd_thrust);
+
+            loop.sleep();
+        }
+    }
+
+private:
+    // Node
+    ros::NodeHandle node;
+
+    // Subscribers
+    ros::Subscriber sub_cmd;
+    ros::Subscriber sub_pose;
+    ros::Subscriber sub_twist;
+    ros::Subscriber sub_pid_vel;
+    ros::Subscriber sub_pid_head;
+
+    // Publishers
+    ros::Publisher pub_pidStateSpeed;
+    ros::Publisher pub_pidCmdSpeed;
+    ros::Publisher pub_pidStateHead;
+    ros::Publisher pub_pidCmdHead;
+    ros::Publisher pub_cmd_thrust;
+
+    // Internal variables
+    std_msgs::Float64 heading;
+    geometry_msgs::Twist cmd_thrust;
+};
+
+int main(int argc, char **argv)
+{
+    // Node initialization
+    std::cout << "Node initialization " << std::endl;
+    ros::init(argc, argv, "PIDHandler");
+
+    PIDHandler pid;
+
+    pid.spin();
+    return 0;
+}
+

helios_control/helios_command/src/algoreg/algo.cpp

@@ -0,0 +1,9 @@
+//
+// Created by simon on 19/06/18.
+//
+
+#include "algo.h"
+
+Algo::Algo() {
+    debug = false;
+}

helios_control/helios_command/src/algoreg/algo.h

@@ -0,0 +1,18 @@
+//
+// Created by simon on 19/06/18.
+//
+
+#pragma once
+
+#include <map>
+#include <vector>
+
+class Algo {
+public:
+    Algo();
+
+    virtual double run(std::vector<double> wp_a, std::vector<double> wp_b, std::vector<double> state_vec)= 0;
+
+    bool debug;
+    std::map<std::string, double> debugMap;
+};

helios_control/helios_command/src/algoreg/lineFollow.cpp

@@ -0,0 +1,78 @@
+//
+// Created by simon on 19/06/18.
+//
+
+#include "lineFollow.h"
+
+LineFollow::LineFollow(){
+
+};
+
+double LineFollow::run(std::vector<double> wp_a, std::vector<double> wp_b, std::vector<double> state_vec){
+
+    double ax = wp_a[0];
+    printf("ax = [%f]\n",ax);
+    double ay = wp_a[1];
+    printf("ay = [%f]\n",ay);
+    double bx = wp_b[0];
+    printf("bx = [%f]\n",bx);
+    double by = wp_b[1];
+    printf("by = [%f]\n",by);
+    const double x = state_vec[0];
+    printf("x = [%f]\n",x);
+    const double y = state_vec[1];
+    printf("y = [%f]\n",y);
+
+    double headLine = atan2(by-ay,bx-ax); // ENU convention, angle from east
+    printf("headLine = [%f]\n",headLine);
+
+    // Si le bateau dépasse la ligne
+    const double angleNextWp2Boat = atan2(y-by,x-bx); // ENU convention, angle from east
+    printf("angleNextWp2Boat = [%f]\n",angleNextWp2Boat);
+    printf("angle_rad(angleNextWp2Boat,-headLine) = [%f]\n",angle_rad(angleNextWp2Boat,-headLine));
+    if(fabs(angle_rad(angleNextWp2Boat,-headLine))<M_PI/2.0){
+        // If boat has crossed the line
+        printf("===================== Inverting WayPoints\n");
+
+        const double tmpx = ax;
+        const double tmpy = ay;
+        ax = bx;
+        ay = by;
+        bx = tmpx;
+        by = tmpy;
+
+        headLine = atan2(by-ay,bx-ax); // ENU convention, angle from east
+    }
+
+    // Calcul de la distance à la ligne, si c'est un point alors une valeur est définie
+    double dist2Line;
+    if( sqrt( pow(bx-ax,2) + pow(by-ay,2)) != 0){
+        dist2Line = ((bx-ax)*(y-ay) - (by-ay)*(x-ax)) / sqrt( pow(bx-ax,2) + pow(by-ay,2));
+    } else{ dist2Line = 100; }
+    printf("dist2Line = [%f]\n",dist2Line);
+
+    // Calcul de l'erreur
+    const double distAtanPt = 3;
+    const double error = atan(dist2Line / distAtanPt);
+    printf("headLine = [%f]\n", headLine);
+    printf("error = [%f]\n", error);
+    printf("headLine - error = [%f]\n", headLine - error);
+
+    // Bornage entre -pi et pi
+    const double wantedHead = angle_rad(headLine,- error);
+    printf("wantedHead = [%f]\n", wantedHead); // Le cap voulu par rapport au repere global
+
+    if(debug){
+        debugMap["wantedHead"] = wantedHead;
+        debugMap["headLine"] = headLine;
+        debugMap["angleNextWp2Boat"] = angleNextWp2Boat;
+        debugMap["error"] = error;
+        debugMap["dist2Line"] = dist2Line;
+        debugMap["ax"] = ax;
+        debugMap["ay"] = ay;
+        debugMap["bx"] = bx;
+        debugMap["by"] = by;
+    }
+
+    return wantedHead;
+}

helios_control/helios_command/src/algoreg/lineFollow.h

@@ -0,0 +1,15 @@
+//
+// Created by simon on 19/06/18.
+//
+
+#pragma once
+
+#include "vector"
+#include "algo.h"
+#include "geodesy/utility.h"
+
+class LineFollow: public Algo {
+public:
+    LineFollow();
+    double run(std::vector<double> wp_a, std::vector<double> wp_b, std::vector<double> state_vec) override;
+};

helios_control/helios_command/src/algoreg/pid.cpp

@@ -0,0 +1,46 @@
+//
+// Created by simon on 20/06/18.
+//
+
+#include <numeric>
+#include "pid.h"
+
+PID::PID(double p, double i, double d, int filterDepth, int intDepth) {
+    this->p = p;
+    this->i = i;
+    this->d = d;
+    this->filterDepth = filterDepth;
+    this->intDepth = intDepth;
+    integratedVal = 0;
+    derivatedVal = 0;
+}
+
+PID::PID(double p, double i, double d): PID::PID(p, i, d, 3, 100){};
+
+double PID::run(double target, double value) {
+    // Handle memory
+    if(mem.size()<filterDepth){
+        mem.insert(mem.begin(),target-value);
+    }
+    else{
+        const double smoothVal = std::accumulate(mem.begin(), mem.end(), 0.0)/mem.size();
+        mem.insert(mem.begin(), target-smoothVal);
+        mem.erase(mem.end());
+    }
+    const double smoothError = mem[0];
+
+    // compute derivatedVal
+    if(mem.size()>=filterDepth){
+        const double derivatedValAfter = mem[0];
+        const double derivatedValBefore = mem[1];
+        derivatedVal = derivatedValAfter - derivatedValBefore;
+    }
+
+    // compute integratedVal
+    integratedVal += mem[0];
+
+    // compute command
+    const double u = p*smoothError + i*integratedVal + d*derivatedVal;
+
+    return u;
+}

helios_control/helios_command/src/algoreg/pid.h

@@ -0,0 +1,22 @@
+//
+// Created by simon on 20/06/18.
+//
+
+#pragma once
+
+#include <vector>
+
+class PID {
+public:
+    PID(double p, double i, double d, int filterDepth, int intDepth);
+    PID(double p, double i, double d): PID(p, i, d, 3, 100);
+    double run(double target, double value);
+    double p;
+    double i;
+    double d;
+    double filterDepth;
+private:
+    std::vector<double> mem;
+    double integratedVal;
+    double derivatedVal;
+};