added velocity_TT controller

path_follower/CMakeLists.txt

@@ -152,6 +152,7 @@ add_library(${PROJECT_NAME} SHARED
     src/controller/robotcontroller_ackermann_modelbased.cpp
     src/controller/robotcontroller_ekm.cpp
     src/controller/robotcontroller_ekm_TT.cpp
+    src/controller/robotcontroller_velocity_TT.cpp
 
     src/local_planner/abstract_local_planner.cpp
     src/local_planner/local_planner_null.cpp

path_follower/include/path_follower/controller/robotcontroller_velocity_TT.h

@@ -0,0 +1,163 @@
+#ifndef ROBOTCONTROLLER_VELOCITY_TT_H
+#define ROBOTCONTROLLER_VELOCITY_TT_H
+
+/// THIRD PARTY
+#include <Eigen/Core>
+#include <opencv2/core/core.hpp>
+#include <pcl_ros/point_cloud.h>
+
+/// PROJECT
+#include <path_follower/controller/robotcontroller.h>
+#include <path_follower/utils/parameters.h>
+
+
+class RobotController_Velocity_TT: public RobotController
+{
+public:
+    /**
+     * @brief RobotController_Kinematic_HBZ
+     */
+    RobotController_Velocity_TT();
+
+    /**
+     * @brief stopMotion stops the robot
+     */
+    virtual void stopMotion();
+    /**
+     * @brief start
+     */
+    virtual void start();
+
+
+protected:
+    /**
+     * @brief computeMoveCommand computes the command velocity for the robot
+     *
+     * The command velocity is computed for each controller differently. This is the core of
+     * every controller. For more details, please visit: https://github.com/cogsys-tuebingen/gerona/wiki/controllers
+     * On this wiki page, you will find references for each controller, where more mathematical and experimental details
+     * can be found.
+     *
+     * @param cmd
+     */
+    virtual MoveCommandStatus computeMoveCommand(MoveCommand* cmd);
+    /**
+     * @brief publishMoveCommand publishes the computed move command
+     *
+     * The command velocity is set depending on the kinematics of the robot. E.g. for
+     * differential drives the command input is (v, w), where v is linear, and w angular velocity,
+     * and for an Ackermann drive, the command input is (v, phi), where v is linear velocity, and
+     * phi is the steering angle. For an omnidirectional vehicle, it is possible to directly set
+     * the linear velocity and the direction angle, while the rotation is set independently.
+     *
+     * @param cmd
+     */
+    virtual void publishMoveCommand(const MoveCommand &cmd) const;
+    /**
+     * @brief initialize
+     */
+    virtual void initialize();
+    /**
+     * @brief WheelVelocities computes the speed of the left and right wheels
+     *
+     * Currently not used, but could be helpful.
+     *
+     */
+    //void WheelVelocities(const std_msgs::Float64MultiArray::ConstPtr& array);
+    /**
+     * @brief computeSpeed computes the actual command velocity, by using different speed control techniques
+     */
+    //virtual double computeSpeed();
+
+private:
+    void findMinDistance();
+
+protected:
+    struct ControllerParameters : public RobotController::ControllerParameters
+    {
+        P<double> dist_thresh;
+        P<double> krep;
+
+
+
+        ControllerParameters(const std::string& name = "ekm"):
+
+            RobotController::ControllerParameters(name),
+            dist_thresh(this, "dist_thresh", 1, "dist_thresh description."),
+            krep(this, "krep", 0.1, "krep description")
+
+        {}
+    } opt_;
+
+    const RobotController::ControllerParameters& getParameters() const
+    {
+        return opt_;
+    }
+
+    struct Command
+    {
+        RobotController_Velocity_TT *parent_;
+
+        //! Speed of the movement.
+        float speed;
+        //! Direction of movement as angle to the current robot orientation.
+        float direction_angle;
+        //! rotational velocity.
+        float rotation;
+
+
+        // initialize all values to zero
+        Command(RobotController_Velocity_TT *parent):
+            parent_(parent),
+            speed(0.0f), direction_angle(0.0f), rotation(0.0f)
+        {}
+
+        operator MoveCommand()
+        {
+            MoveCommand mcmd(true);
+            mcmd.setDirection(direction_angle);
+            mcmd.setVelocity(speed);
+            mcmd.setRotationalVelocity(rotation);
+            return mcmd;
+        }
+
+        bool isValid()
+        {
+            if ( std::isnan(speed) || std::isinf(speed)
+                 || std::isnan(direction_angle) || std::isinf(direction_angle)
+                 || std::isnan(rotation) || std::isinf(rotation) )
+            {
+                ROS_FATAL("Non-numerical values in command: %d,%d,%d,%d,%d,%d",
+                          std::isnan(speed), std::isinf(speed),
+                          std::isnan(direction_angle), std::isinf(direction_angle),
+                          std::isnan(rotation), std::isinf(rotation));
+                // fix this instantly, to avoid further problems.
+                speed = 0.0;
+                direction_angle = 0.0;
+                rotation = 0.0;
+
+                return false;
+            } else {
+                return true;
+            }
+        }
+    };
+
+    Command cmd_;
+
+
+    void reset();
+    void setPath(Path::Ptr path);
+
+    cv::Vec2f CalcForceRep();
+    cv::Vec2f CalcForceRep(const pcl::PointCloud<pcl::PointXYZ>& cloud);
+
+private:
+
+    std::queue<geometry_msgs::PoseStamped> targetPoses_;
+
+
+
+};
+
+#endif // ROBOTCONTROLLER_KINEMATIC_HBZ_H

path_follower/src/controller/robotcontroller_velocity_TT.cpp

@@ -0,0 +1,170 @@
+// HEADER
+#include <path_follower/controller/robotcontroller_velocity_TT.h>
+
+// THIRD PARTY
+
+
+// PROJECT
+#include <path_follower/factory/controller_factory.h>
+#include <path_follower/utils/pose_tracker.h>
+#include <path_follower/collision_avoidance/collision_avoider.h>
+#include <path_follower/utils/obstacle_cloud.h>
+
+
+// SYSTEM
+#include <cmath>
+#include <opencv2/core/core.hpp>
+#include <pcl_ros/transforms.h>
+
+
+REGISTER_ROBOT_CONTROLLER(RobotController_Velocity_TT, velocity_TT, default_collision_avoider);
+
+//using namespace Eigen;
+
+
+RobotController_Velocity_TT::RobotController_Velocity_TT():
+    RobotController(),
+    cmd_(this)
+{
+
+
+
+}
+
+void RobotController_Velocity_TT::stopMotion()
+{
+
+    cmd_.speed = 0;
+    cmd_.direction_angle = 0;
+    cmd_.rotation = 0;
+
+    MoveCommand mcmd = cmd_;
+    publishMoveCommand(mcmd);
+}
+
+void RobotController_Velocity_TT::initialize()
+{
+    RobotController::initialize();
+
+    //reset the index of the current point on the path
+
+
+
+}
+
+
+
+void RobotController_Velocity_TT::start()
+{
+    RobotController::start();
+}
+
+void RobotController_Velocity_TT::reset()
+{
+    RobotController::reset();
+
+}
+
+void RobotController_Velocity_TT::setPath(Path::Ptr path)
+{
+    RobotController::setPath(path);
+
+}
+
+/*
+double RobotController_EKM::computeSpeed()
+{
+
+}
+*/
+cv::Vec2f RobotController_Velocity_TT::CalcForceRep(const pcl::PointCloud<pcl::PointXYZ>& cloud)
+{
+    float min_dist = opt_.dist_thresh();
+    float curDist = 0;
+    float t1,krep = opt_.krep();
+    float min_dist_inv = 1.0f/min_dist;
+    cv::Vec2f t2,tfrep;
+    cv::Vec2f curP(0,0);
+    cv::Vec2f sumF(0,0);
+    int counter = 0;
+
+
+    for(const pcl::PointXYZ& pt : cloud) {
+        curP[0] = pt.x;
+        curP[1] = pt.y;
+
+        curDist = std::sqrt(curP.dot(curP));
+        if(curDist < min_dist){
+
+            t1 = (1.0f/curDist - min_dist_inv);
+            t2 = (1.0f/(curDist*curDist))* ((-curP)*min_dist_inv);
+            tfrep = krep*t1*t2;
+            sumF = sumF+tfrep;
+            counter++;
+        }
+
+    }
+    cv::Vec2f result = sumF * (1.0f/(float)counter);
+    return result;
+}
+
+cv::Vec2f RobotController_Velocity_TT::CalcForceRep()
+{
+    auto obstacle_cloud = collision_avoider_->getObstacles();
+    const pcl::PointCloud<pcl::PointXYZ>& cloud = *obstacle_cloud->cloud;
+
+    if(cloud.header.frame_id == PathFollowerParameters::getInstance()->robot_frame()) {
+        return CalcForceRep(cloud);
+    }
+    else
+    {
+        if (!pose_tracker_->getTransformListener().waitForTransform(PathFollowerParameters::getInstance()->robot_frame(),
+                                                                    obstacle_cloud->getFrameId(),
+                                                                    obstacle_cloud->getStamp(),
+                                                                    ros::Duration(0.1) ))
+        {
+            ROS_ERROR_THROTTLE_NAMED(1, "velocity_TT", "cannot transform cloud to robotframe");
+            return cv::Vec2f(0,0);
+
+        }
+        pcl::PointCloud<pcl::PointXYZ> tcloud;
+
+        pcl_ros::transformPointCloud(PathFollowerParameters::getInstance()->robot_frame(),
+                cloud,
+                tcloud,
+                pose_tracker_->getTransformListener()
+            );
+        return CalcForceRep(tcloud);
+    }
+
+}
+
+
+
+RobotController::MoveCommandStatus RobotController_Velocity_TT::computeMoveCommand(MoveCommand *cmd)
+{
+    // omni drive can rotate.
+    *cmd = MoveCommand(true);
+
+    if(path_interpol.n() < 2) {
+        ROS_ERROR("[Line] path is too short (N = %d)", (int) path_interpol.n());
+
+        stopMotion();
+        return MoveCommandStatus::REACHED_GOAL;
+    }
+
+    return MoveCommandStatus::OKAY;
+
+
+}
+
+void RobotController_Velocity_TT::publishMoveCommand(const MoveCommand &cmd) const
+{
+    geometry_msgs::Twist msg;
+    msg.linear.x  = cmd.getVelocity();
+    msg.linear.y  = 0;
+    msg.angular.z = cmd.getRotationalVelocity();
+
+    cmd_pub_.publish(msg);
+}
+