Merge pull request #22 from Heenne/fpc-enhancement

Fpc enhancement

Author: Heenne

fpc_ros/CMakeLists.txt

@@ -131,8 +131,13 @@ include_directories(
 ## Declare a C++ library
 set(FPC_DATA_CLASSES_SOURCES
 
-	include/fpc_ros/data_classes/local_planner_robot_info.hpp
-	include/fpc_ros/data_classes/lyapunov_params.hpp)
+	include/fpc_ros/data_classes/fpc_param_info.hpp
+	include/fpc_ros/data_classes/fpc_robot_info.hpp
+	include/fpc_ros/data_classes/lyapunov_params.hpp
+	include/fpc_ros/data_classes/controller_params.hpp
+	
+	include/fpc_ros/data_classes/fpc_param_manager.h
+	src/data_classes/fpc_param_manager.cpp)
 add_library(fpc_data_classes ${FPC_DATA_CLASSES_SOURCES})
 
 set(FPC_ROS_SOURCES

fpc_ros/include/fpc_ros/data_classes/controller_params.hpp

@@ -0,0 +1,22 @@
+#pragma once
+
+namespace fpc_data_classes
+{
+	/**
+     * @brief Contains all settings of the controller that calculates the movement of the robot
+     * 
+     */
+	struct ControllerParams
+	{
+		//! This calue defines the frequency the controller is called
+		float controller_frequency;
+		//! Max linear forward velocity of the mobile robot
+		float max_vel_x;
+		//! Min linear velocity of the mobile robot (if backwards movement are allowed, this is negative)
+		float min_vel_x;
+		//! Max rotational movement of the mobile robot (no differentiation between left and right movement)
+		float max_vel_theta;
+		//! Min rotational movement of the mobile robot (no differentiation between left and right movement)
+		float min_vel_theta;
+	};
+}

fpc_ros/include/fpc_ros/data_classes/fpc_param_info.hpp

@@ -0,0 +1,41 @@
+#pragma once 
+
+#include "ros/ros.h"
+
+#include <string>
+#include <vector>
+#include <memory>
+
+#include <fpc_ros/data_classes/fpc_robot_info.hpp>
+#include <fpc_ros/data_classes/lyapunov_params.hpp>
+
+namespace fpc_data_classes
+{
+	struct FPCParamInfo
+	{
+		//! Parameter is defining the tolerance when arriving at the goal in the x and y direction
+		double xy_default_tolerance;
+		//! Parameter is defining the tolerance when arriving at the goal for the orientation.
+		double yaw_default_tolerance;
+		
+		//! Prune the plan so every position of the original plan that was passed 1m ago will be deleted
+		bool prune_plan;
+		//! Params for the controller
+		fpc_data_classes::ControllerParams controller_params;
+		
+		//! Param info for all robots included in the formation
+		std::vector<std::shared_ptr<fpc_data_classes::FPCRobotInfo>> robot_info_list;
+		//! Param info for the current robot
+		std::shared_ptr<fpc_data_classes::FPCRobotInfo> current_robot_info;
+		//! Param info for the master robot
+		std::shared_ptr<fpc_data_classes::FPCRobotInfo> master_robot_info;
+
+		std::string getCurrentRobotName() { return this->current_robot_info->robot_name; }
+		std::string getCurrentRobotNamespace() { return this->current_robot_info->robot_namespace; }
+		std::string getCurrentRobotPoseTopic() { return this->current_robot_info->robot_pose_topic; }
+		std::string getCurrentRobotGroundTruthTopic() { return this->current_robot_info->robot_ground_truth_topic; }
+		std::string getCurrentRobotOdomTopic() { return this->current_robot_info->robot_odom_topic; }
+		std::string getCurrentRobotCmdVelTopic() { return this->current_robot_info->robot_cmd_vel_topic; }
+		LyapunovParams getLyapunovParams() { return this->current_robot_info->lyapunov_params; }
+	};
+}

fpc_ros/include/fpc_ros/data_classes/fpc_param_manager.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "ros/ros.h"
+
+#include <string>
+#include <memory>
+#include <fpc_ros/data_classes/fpc_param_info.hpp>
+
+namespace fpc_data_classes
+{
+	class FPCParamManager
+	{
+		public:
+			FPCParamManager(ros::NodeHandle &nh, ros::NodeHandle &controller_nh);
+
+			void getParams();
+
+			std::shared_ptr<FPCParamInfo> getLocalPlannerInfo();
+
+		private:
+			ros::NodeHandle &nh_;
+			ros::NodeHandle &controller_nh_;
+
+			std::shared_ptr<FPCParamInfo> fpc_param_info_;
+
+			double getNumberFromXMLRPC(XmlRpc::XmlRpcValue value, const std::string full_param_name);
+	};
+}

…ata_classes/local_planner_robot_info.hpp …/fpc_ros/data_classes/fpc_robot_info.hppfpc_ros/include/fpc_ros/data_classes/local_planner_robot_info.hpp renamed to fpc_ros/include/fpc_ros/data_classes/fpc_robot_info.hpp fpc_ros/include/fpc_ros/data_classes/local_planner_robot_info.hpp renamed to fpc_ros/include/fpc_ros/data_classes/fpc_robot_info.hpp

@@ -2,6 +2,7 @@
 
 #include <string>
 #include <fpc_ros/data_classes/lyapunov_params.hpp>
+#include <fpc_ros/data_classes/controller_params.hpp>
 
 namespace fpc_data_classes
 {
@@ -10,7 +11,7 @@ namespace fpc_data_classes
      * Data is read from the local_planner_config.yaml and loaded by move_base_flex
      * 
      */
-	struct LocalPlannerRobotInfo
+	struct FPCRobotInfo
 	{
 		//! Name of the robot
 		std::string robot_name;
@@ -20,8 +21,12 @@ namespace fpc_data_classes
 		bool fpc_master;
 		//! Topic name where the pose of the robot can be received
 		std::string robot_pose_topic;
+		//! Topic name where the ground truth pose of the robot will be published from Gazebo
+		std::string robot_ground_truth_topic;
 		//! Topic name where the odometry of the robot can be received
 		std::string robot_odom_topic;
+		//! Topic name where the velocity of the robot can be published to
+		std::string robot_cmd_vel_topic;
 		//! Params for the lyapunov controller
 		fpc_data_classes::LyapunovParams lyapunov_params;
 	};

fpc_ros/include/fpc_ros/fpc_controller_base.h

@@ -6,6 +6,7 @@
 #include <geometry_msgs/Pose.h>
 #include <geometry_msgs/PoseStamped.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
+#include <geometry_msgs/Twist.h>
 #include <nav_msgs/Odometry.h>
 #include <tf2_ros/buffer.h>
 #include <tf/transform_listener.h>
@@ -15,7 +16,7 @@
 #include <vector>
 #include <eigen3/Eigen/Dense>
 
-#include <fpc_ros/data_classes/local_planner_robot_info.hpp>
+#include <fpc_ros/data_classes/fpc_param_info.hpp>
 #include <fpp_msgs/LocalPlannerMetaData.h>
 
 namespace fpc
@@ -25,8 +26,7 @@ namespace fpc
 		public:
 			#pragma region Constructors
 			FPCControllerBase(
-				std::vector<std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo>> &robot_info_list,
-				std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo> &robot_info,
+				std::shared_ptr<fpc_data_classes::FPCParamInfo> fpc_param_info,
 				ros::NodeHandle &nh,
 				ros::NodeHandle &controller_nh);
 			#pragma endregion
@@ -46,7 +46,7 @@ namespace fpc
 													 geometry_msgs::TwistStamped &cmd_vel,
 													 std::string &message);
 
-			virtual void publishMetaData();
+			virtual void publishMetaData(geometry_msgs::Pose2D target_pose);
 			#pragma endregion
 
 			#pragma region Getter/Setter
@@ -76,41 +76,54 @@ namespace fpc
 			std::string robot_ns_;
 
 			#pragma region ProcessInfo
-			//! This is all the information that was read from the config file about each robot
-            std::vector<std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo>> &robot_info_list_;
-			//! This points to the object that contains the information about this robot
-            std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo> &robot_info_;
-			//! This point to the object that represents the master in the formation path planner
-			const std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo> master_robot_info_;
+			std::shared_ptr<fpc_data_classes::FPCParamInfo> fpc_param_info_;
+			// //! This is all the information that was read from the config file about each robot
+            // std::vector<std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo>> &robot_info_list_;
+			// //! This points to the object that contains the information about this robot
+            // std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo> &robot_info_;
+			// //! This point to the object that represents the master in the formation path planner
+			// const std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo> master_robot_info_;
+
+			geometry_msgs::Twist last_published_cmd_vel_;
 
 			std::vector<geometry_msgs::PoseStamped> global_plan_;
 
-			int pose_index_;
+			float velocity_factor_;
 			bool controller_finished_;
 
 			geometry_msgs::PoseStamped last_target_pose_;
 			#pragma endregion
 
-
-
-			#pragma region Topics/Services/Actions
+			#pragma region Topics/Services/Actions/Timers
 			ros::Subscriber robot_amcl_pose_subscriber_;
 			geometry_msgs::Pose current_robot_amcl_pose_;
 			ros::Subscriber robot_odom_subscriber_;
 			nav_msgs::OdometryConstPtr current_robot_odom_;
 			ros::Subscriber robot_ground_truth_subscriber_; // This subscriber will only work in Gazebo where ground truth is published
 			nav_msgs::OdometryConstPtr current_robot_ground_truth_;
 
+			ros::Publisher cmd_vel_publisher_;
 			ros::Publisher meta_data_publisher_;
 			fpp_msgs::LocalPlannerMetaData meta_data_msg_; // This is the msg that will be published. Every info can be stored here and will be reset when message is published. 
+
+			ros::Timer controller_timer_;
 			#pragma endregion
 
-			#pragma CallbackMethods
+			#pragma region CallbackMethods
 			void getRobotPoseCb(const geometry_msgs::PoseWithCovarianceStampedConstPtr &msg);
 			void getRobotOdomCb(const nav_msgs::OdometryConstPtr &msg);
 
 			void getRobotGroundTruthCb(const nav_msgs::OdometryConstPtr &msg);
 
+			void onControllerTimerCB(const ros::TimerEvent& timer_event_info);
+			#pragma endregion
+
+			#pragma region Controller Methods
+			virtual void runController();
+			float calcLinVelocity(geometry_msgs::Pose2D diff_vector, float scale_factor);
+			float calcRotVelocity(geometry_msgs::Pose2D diff_vector);
+
+			int locateRobotOnPath(geometry_msgs::Pose current_pose);
 			#pragma endregion
 
 			#pragma region ProtectedHelperMethods
@@ -130,12 +143,27 @@ namespace fpc
              */
             virtual void initTimers();
 
-			std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo> getMasterRobotInfo(
-				const std::vector<std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo>> &robot_info_list);
-
 			geometry_msgs::Pose2D convertPose(geometry_msgs::Pose pose_to_convert);
+			Eigen::Vector3f convertPose(geometry_msgs::Pose2D pose_to_convert);
+			geometry_msgs::Pose2D calcDiff(geometry_msgs::Pose start_pose, geometry_msgs::Pose end_pose);
 			geometry_msgs::Pose2D calcDiff(geometry_msgs::Pose2D start_pose, geometry_msgs::Pose2D end_pose);
 			geometry_msgs::Twist calcDiff(geometry_msgs::Twist start_vel, geometry_msgs::Twist end_vel);
-			#pragma endregion
+			float calcEuclideanDiff(geometry_msgs::Pose point1, geometry_msgs::Pose point2);
+			float calcEuclideanDistance(geometry_msgs::Pose2D vector);
+
+			float calcVelocityFactor(geometry_msgs::Pose2D pose_diff, float largest_euclidean_diff);
+
+			/**
+             * @brief Create a transformation matrix to transform points from and to the geometry cs of this object
+             * 
+             * @param lead_vector_world_cs translation from the world to this cs
+             * @param rotation Rotation form the world to this cs. Rotation only uses yaw rotation.
+             * @return Eigen::Matrix<float, 4, 4> Transformation matrix in the x/y area and yaw rotation.
+             */
+            Eigen::Matrix<float, 4, 4> createTransformationMatrix(Eigen::Vector2f lead_vector_world_cs, float rotation);
+			Eigen::Vector2f transformVector(Eigen::Vector2f vector_to_transform,
+											Eigen::Vector2f lead_vector,
+											float rotation);
+#pragma endregion
 	};
 }

fpc_ros/include/fpc_ros/fpc_controller_master.h

@@ -4,21 +4,50 @@
 
 #include <memory>
 #include <vector>
+#include <map>
 
 #include <fpc_ros/fpc_controller_base.h>
-#include <fpc_ros/data_classes/local_planner_robot_info.hpp>
+#include <fpp_msgs/NextTargetPoseCommand.h>
 
 namespace fpc
 {
 	class FPCControllerMaster : public fpc::FPCControllerBase
 	{
 		public:
 			FPCControllerMaster(
-				std::vector<std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo>> &robot_info_list,
-				std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo> &robot_info,
+				std::shared_ptr<fpc_data_classes::FPCParamInfo> fpc_param_info,
 				ros::NodeHandle &nh,
 				ros::NodeHandle &controller_nh);
 
+			bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) override;
+
 		private:
+			int pose_index_;
+			
+			std::map<std::string, std::shared_ptr<ros::ServiceClient>> next_target_command_clt_list_;
+			std::map<std::string, fpp_msgs::NextTargetPoseCommand::Response> saved_command_res_list_;
+
+
+
+			void runController() override;
+
+			/**
+             * @brief Helper method for intializing all services
+             * 
+             */
+            void initServices() override;
+
+
+			#pragma region Callback Methods
+			#pragma endregion
+
+			#pragma region Helper Methods
+			bool allSlavesReady();
+			bool isSlaveReady(std::string robot_name);
+			float getLargestDiff();
+			// int getHighestPoseIndex();
+			// int getLowestPoseIndex();
+			// fpp_msgs::FPCRobotScaleInfo getHighestLinScaleValue(int next_target_pose);
+			#pragma endregion
 	};
 }

fpc_ros/include/fpc_ros/fpc_controller_slave.h

@@ -3,18 +3,36 @@
 #include "ros/ros.h"
 
 #include <fpc_ros/fpc_controller_base.h>
+#include <fpp_msgs/NextTargetPoseCommand.h>
 
 namespace fpc
 {
 	class FPCControllerSlave : public fpc::FPCControllerBase
 	{
 		public:
 			FPCControllerSlave(
-				std::vector<std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo>> &robot_info_list,
-				std::shared_ptr<fpc_data_classes::LocalPlannerRobotInfo> &robot_info,
+				std::shared_ptr<fpc_data_classes::FPCParamInfo> fpc_param_info,
 				ros::NodeHandle &nh,
 				ros::NodeHandle &controller_nh);
 
 		private:
+			ros::ServiceServer next_target_command_srv_;
+			fpp_msgs::NextTargetPoseCommand::Request saved_target_cmd_req_;
+
+			geometry_msgs::Pose2D diff_vector_;
+
+			/**
+             * @brief Helper method for intializing all services
+             * 
+             */
+            void initServices() override;
+
+			void runController() override;
+
+			#pragma region Callback Methods
+			bool onNextTargetCommand(fpp_msgs::NextTargetPoseCommand::Request &req,
+									 fpp_msgs::NextTargetPoseCommand::Response &res);
+
+			#pragma endregion
 	};
 }