| 81 | |
| 82 | |
| 83 | prometheus_msgs::PositionReference Controller_Test::Circle_trajectory_generation(float time_from_start) |
| 84 | { |
| 85 | prometheus_msgs::PositionReference Circle_trajectory; |
| 86 | float omega; |
| 87 | if( circle_radius != 0) |
| 88 | { |
| 89 | omega = direction * fabs(linear_vel / circle_radius); |
| 90 | }else |
| 91 | { |
| 92 | omega = 0.0; |
| 93 | } |
| 94 | const float angle = time_from_start * omega; |
| 95 | const float cos_angle = cos(angle); |
| 96 | const float sin_angle = sin(angle); |
| 97 | |
| 98 | // cout << "omega : " << omega * 180/M_PI <<" [deg/s] " <<endl; |
| 99 | // cout << "angle : " << angle * 180/M_PI <<" [deg] " <<endl; |
| 100 | |
| 101 | Circle_trajectory.header.stamp = ros::Time::now(); |
| 102 | |
| 103 | Circle_trajectory.time_from_start = time_from_start; |
| 104 | |
| 105 | Circle_trajectory.Move_mode = prometheus_msgs::PositionReference::TRAJECTORY; |
| 106 | |
| 107 | Circle_trajectory.position_ref[0] = circle_radius * cos_angle + circle_center[0]; |
| 108 | Circle_trajectory.position_ref[1] = circle_radius * sin_angle + circle_center[1]; |
| 109 | Circle_trajectory.position_ref[2] = circle_center[2]; |
| 110 | |
| 111 | Circle_trajectory.velocity_ref[0] = - circle_radius * omega * sin_angle; |
| 112 | Circle_trajectory.velocity_ref[1] = circle_radius * omega * cos_angle; |
| 113 | Circle_trajectory.velocity_ref[2] = 0; |
| 114 | |
| 115 | Circle_trajectory.acceleration_ref[0] = - circle_radius * pow(omega, 2.0) * cos_angle; |
| 116 | Circle_trajectory.acceleration_ref[1] = - circle_radius * pow(omega, 2.0) * sin_angle; |
| 117 | Circle_trajectory.acceleration_ref[2] = 0; |
| 118 | |
| 119 | // Circle_trajectory.jerk_ref[0] = circle_radius * pow(omega, 3.0) * sin_angle; |
| 120 | // Circle_trajectory.jerk_ref[1] = - circle_radius * pow(omega, 3.0) * cos_angle; |
| 121 | // Circle_trajectory.jerk_ref[2] = 0; |
| 122 | |
| 123 | // Circle_trajectory.snap_ref[0] = circle_radius * pow(omega, 4.0) * cos_angle; |
| 124 | // Circle_trajectory.snap_ref[1] = circle_radius * pow(omega, 4.0) * sin_angle; |
| 125 | // Circle_trajectory.snap_ref[2] = 0; |
| 126 | |
| 127 | Circle_trajectory.yaw_ref = 0; |
| 128 | // Circle_trajectory.yaw_rate_ref = 0; |
| 129 | // Circle_trajectory.yaw_acceleration_ref = 0; |
| 130 | |
| 131 | return Circle_trajectory; |
| 132 | } |
| 133 | |
| 134 | prometheus_msgs::PositionReference Controller_Test::Eight_trajectory_generation(float time_from_start) |
| 135 | { |