ROS_Begin

1. Turtlesim Try

rospack find turtlesim

Begin

roscore
rosrun turtlesim turtlesim_node
rosrun turtlesim turtle_teleop_key
//The three commands must be in three shells.


2. Create a workspace

echo $ROS_PACKAGE_PATH

mkdir -p ~/dev/catkin_ws/src
cd ~/dev/catkin_ws/src
catkin_init_workspace

cd ~/dev/catkin_ws
catkin_make

source devel/setup.bash     
//在新终端下无法定位功能包时,可运行此命令解决,因此可将此命令写入.bashrc中彻底解决问题。


3. Create a package and build it

cd ~/dev/catkin_ws/src
catkin_create_pkg tutorials std_msgs roscpp
//catkin_create_pkg [package_name] [depend1] [depend2] [depend...]

cd ~/dev/catkin_ws
catkin_make     //build all
//catkin_make tutorials //build tutorials only
#include "Solver/solver_mpc.h" #include "ros/ros.h" #include <cmath> namespace ns_control { void Solver_mpc::solve() { using CppAD::AD;//自动微分 typedef CPPAD_TESTVECTOR(double) Dvector;//向量 const int N = param_.N; const int predicition_domain = N; const int control_domain = N - 1; const double dt = param_.dt; const double car_length = param_.car_length; const int x_range_begin = 0; const int y_range_begin = x_range_begin + predicition_domain; const int yaw_range_begin = y_range_begin + predicition_domain; const int v_range_begin = yaw_range_begin + prediciton_domain; const int lateral_error_range_begin = v_range_begin + predicition_domain; const int yaw_error_range_begin = lateral_error_range_begin + predicition_domain;//横向误差 const int steering_angle_range_begin = yaw_error_range_begin + predicition_domain;//偏航误差 const int throttle_range_begin = steering_angle_range_begin + control_domain; double vehicle_v = state_.vehicle_v; double steering_angle = state_.steering_angle; double throttle = state_.throttle;//功率 int state_size = 6; int control_size = 2; int n_vars = state_size*predicition_domain + control_size*control_domain; int n_constraints = state_size * predicition_domain; Dvector vars(n_vars); for (int i = 0; i < n_vars; i++) { vars[i] = 0; } vars[x_range_begin] = 0.0; vars[y_range_begin] = 0.0; vars[yaw_range_begin] = 0.0; vars[v_range_begin] = vehicle_v; vars[lateral_error_range_begin] = 0.0; vars[yaw_error_range_begin] = 0.0; vars[steering_angle_range_begin] = 0.0; vars[throttle_range_begin] = 0.0; Dvector vars_lowerbound(n_vars); Dvector vars_upperbound(n_vars); for (int i = 0; i < steering_angle_range_begin; i++) { vars_lowerbound[i] = -1.0e19; vars_upperbound[i] = 1.0e19; } for (int i = steering_angle_range_begin; i < throttle_range_begin; i++) { vars_lowerbound[i] = -0.314; //18度 vars_upperbound[i] = 0.314; } for (int i = throttle_range_begin; i < n_vars; i++) { vars_lowerbound[i] = -1.0; vars_upperbound[i] = +1.0; } Dvector constraints_lowerbound(n_constraints); Dvector constraints_upperbound(n_constraints); for (int i = 0; i < n_constraints; i++) { constraints_lowerbound[i] = 0.0; constraints_upperbound[i] = 0.0; } constraints_lowerbound[x_range_begin] = 0.0; constraints_upperbound[x_range_begin] = 0.0; constraints_lowerbound[y_range_begin] = 0.0; constraints_upperbound[y_range_begin] = 0.0; constraints_lowerbound[yaw_range_begin] = 0.0; constraints_upperbound[yaw_range_begin] = 0.0; constraints_lowerbound[v_range_begin] = vehicle_v; constraints_upperbound[v_range_begin] = vehicle_v; constraints_lowerbound[lateral_error_range_begin] = 0.0; constraints_upperbound[lateral_error_range_begin] = 0.0; constraints_lowerbound[yaw_error_range_begin] = 0.0; constraints_upperbound[yaw_error_range_begin] = 0.0; std::string options; bool ok = true; options += "Integer print_level 0\n"; options += "Sparse true forward\n"; options += "Sparse true reverse\n"; options += "Numeric max_cpu_time 0.5\n"; CppAD::ipopt::solve_result<Dvector> solution; FG_eval fg_eval(trajectory_); CppAD::ipopt::solve<Dvector, FG_eval>(options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound, constraints_upperbound, fg_eval, solution); ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success; double cost = solution.obj_value; for (int i = 0; i < N; i++) { geometry_msgs::Point p; p.x = solution.x[x_range_begin + i]; p.y = solution.x[y_range_begin + i]; } controlCommand_.steering_angle.data = solution.x[steering_angle_range_begin]; controlCommand_.throttle.data = solution.x[throttle_range_begin]; std::cout << " steering angle : " << controlCommand_.steering_angle.data << std::endl; std::cout << " throttle angle : " << controlCommand_.throttle.data << std::endl; predictive_path.clear(); TrajectoryPoint p_tmp; for (int i = 0; i < N; i++) { geometry_msgs::Point p; p_tmp.track_x = solution.x[x_range_begin + i]; p_tmp.track_y = solution.x[y_range_begin + i]; predictive_path.push_back(p_tmp); } } }
07-18
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值