Const&Initial

常量类与配置初始化
public final class Const
{
	private static final String _am = "am";
	private static final String _pm = "pm";
	private static final String _eve = "extra_eve";
	private static final String _srcFormat = "src_format";
	private static final String _sheet_index = "sheet_index";
	private static final String _data_index = "data_index";
	private static final String _beginStatistic = "beginStatistic";
	private static GregorianCalendar c = new GregorianCalendar();
	
	private static Logger log = Logger.getLogger(Const.class);
	
	public static final SimpleDateFormat timeFormat = new SimpleDateFormat("HH:mm");
	public static SimpleDateFormat srcFormat;
	public static int beginStatistic;
	public static int sheet_index;
	public static int data_index;
	public static Calendar am = (Calendar) c.clone();
	public static Calendar pm = (Calendar) c.clone();
	public static Calendar eve = (Calendar) c.clone();
	
	static
	{
		try
		{
			srcFormat = new SimpleDateFormat(Initial.getString(_srcFormat));
			beginStatistic = Initial.getInt(_beginStatistic);
			sheet_index = Initial.getInt(_sheet_index);
			data_index = "abcdef".indexOf(Initial.getString(_data_index).toLowerCase());
			am.setTime(timeFormat.parse(Initial.getString(_am)));
			pm.setTime(timeFormat.parse(Initial.getString(_pm)));
			eve.setTime(timeFormat.parse(Initial.getString(_eve)));
		}
		catch (Exception e)
		{
			log.error(e);
		}
	}
	
	
}

Initial
public class Initial
{
	private static Properties pros = new Properties();
	private static Logger log = Logger.getLogger(Initial.class);
	
	static
	{
		try
		{
			pros.load(new FileInputStream("conf/jxl.properties"));
		}
		catch (Exception e)
		{
			log.error(e);
		}
	}
	
	public static String getString(String para)
	{
		if (para == null || para.length() == 0)
		{
			return "";
		}
		return pros.get(para).toString().trim();
	}

	public static int getInt(String para)
	{
		return Integer.valueOf(getString(para));
	}
	
	public static void main(String[] args) 
	{
		pros.list(System.out);
	}
	
}

 

#ifndef MOTION_CONTROLLER_SEPARATE_H #define MOTION_CONTROLLER_SEPARATE_H #include <cmath> #include <ros/ros.h> #include <sensor_msgs/Imu.h> #include <nav_msgs/Odometry.h> #include <mavros_msgs/RCIn.h> #include <mavros_msgs/AttitudeTarget.h> #include <Eigen/Core> #include <Eigen/Geometry> #include <ctrl/pid_controller.h> #include <common/quat_euler_convert.hpp> #include <rofly_ground_ctrl/MotionCtrlDebugSeparate.h> // msg class MotionControllerSeparate { public: // Constructor MotionControllerSeparate() = delete; MotionControllerSeparate(const ros::NodeHandle &nh, const PIDParams &position_pid_params, const PIDParams &velocity_pid_params, const CommonParams &common_params); MotionControllerSeparate(const MotionControllerSeparate &rhs) = delete; MotionControllerSeparate &operator=(const MotionControllerSeparate &rhs) = delete; MotionControllerSeparate(MotionControllerSeparate &&rhs) = delete; MotionControllerSeparate &operator=(MotionControllerSeparate &&rhs) = delete; virtual ~MotionControllerSeparate() {} std::pair<float, float> run(const Eigen::Vector2f &desired_position_inertial, const Eigen::Vector2f &current_position_inertial, const Eigen::Vector2f &feedforward_velocity, const Eigen::Vector2f &current_velocity, const float &feedforward_pitch_angle, const float &feedforward_yaw_angle, const float &current_yaw_angle, const bool &use_thrust_change, float &thrust_return); std::pair<float, float> run(const Eigen::Vector2f &desired_position_inertial, const Eigen::Vector2f &current_position_inertial, const Eigen::Vector2f &feedforward_velocity, const Eigen::Vector2f &current_velocity, const float &feedforward_pitch_angle, const float &feedforward_yaw_angle, const float &current_yaw_angle, const float &traj_duration, const float &current_time, const bool &use_thrust_change, float &thrust_return); bool getFollowTrajFinished() { return follow_traj_finished_; } void resetIntegral() { position_x_pid_.resetIntegral(); position_y_pid_.resetIntegral(); velocity_pid_.resetIntegral(); } private: // ROS NodeHandle ros::NodeHandle nh_; // Publisher ros::Publisher debug_pub_; // Topic std::string debug_topic_; // PID Controller PIDController position_x_pid_; // 外环:位置x -> 期望速度x PIDController position_y_pid_; // 外环:位置y -> 期望速度y PIDController velocity_pid_; // 内环:速度 -> 期望pitch // Variable CommonParams common_params_; float max_velocity_ = 1.0; float max_pitch_angle_ = 0.628; float max_yaw_angle_change_rate_ = 0.174; float dt_ = 0.02; float mass_ = 2.0; float thrust_min_ = 2.0; float thrust_max_ = 12.0; bool first_flag_ = true; float pre_desired_yaw_angle_ = 0.0; bool follow_traj_finished_ = false; float initial_yaw_percentage_ = 0.1; float initial_yaw_velocity_threshold_ = 0.5; // Debug Message rofly_ground_ctrl::MotionCtrlDebugSeparate debug_msg_; // Functions void init(); float constrain(float value, float min_val, float max_val); Eigen::Vector2f transformByYaw(const Eigen::Vector2f &vec_inertial, const float &yaw_angle); public: typedef std::unique_ptr<MotionControllerSeparate> Ptr; }; #endif // MOTION_CONTROLLER_SEPARATE_H 注释上述代码并保留原有注释内容
09-20
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值