void rotation_mouse_hook(t_env *e, int x)
{
if (x < CASE_W * 6)
rotation_left(e);
else if (x < CASE_W * 7)
rotation_right(e);
}
splay_tree rotation_to_root(splay_tree t, int data){
assert(t!=NULL);
if (t->data == data){ return t;}
else if (data > t->data){
if (t->right->data == data){
return rotation_left(t);}
else {
splay_tree t_right = rotation_to_root(t->right,data);
t->right=t_right;
return rotation_left(t);
}
}
else{
if (t->left->data == data){
return rotation_right(t);}
else{
splay_tree t_left=rotation_to_root(t->left,data);
t->left=t_left;
return rotation_right(t);
}
}
}
void generateVisualizationMarker(double steering_angle_rad, visualization_msgs::MarkerArray& marker_array)
{
double turn_radius = 0.0;
if (std::abs(steering_angle_rad) > 0.000001){
// use cotangent
//turn_radius = std::tan((M_PI*0.5 - steering_angle_rad) * p_wheel_base_);
turn_radius = (1.0/std::tan(steering_angle_rad)) * p_wheel_base_;
}
// (0,0) is rear axle middle
// x axis towards front, y axis to left of car
Eigen::Vector2d icc (0.0, turn_radius);
Eigen::Vector2d left_wheel (p_wheel_base_, p_wheel_track_*0.5);
Eigen::Vector2d right_wheel(p_wheel_base_, -p_wheel_track_*0.5);
double dist_left = (left_wheel - icc).norm();
double dist_right = (right_wheel - icc).norm();
double steer_angle_left = std::asin(p_wheel_base_/dist_left);
double steer_angle_right = std::asin(p_wheel_base_/dist_right);
if (turn_radius < 0){
steer_angle_left = -steer_angle_left;
steer_angle_right = -steer_angle_right;
}
ROS_DEBUG("turn radius: %f dist left: %f right: %f", turn_radius, dist_left, dist_right);
ROS_DEBUG("steer angle left: %f right: %f", steer_angle_left, steer_angle_right);
marker_array.markers[0].points.resize(40);
marker_array.markers[1].points.resize(40);
std::vector<geometry_msgs::Point>& point_vector_left = marker_array.markers[0].points;
std::vector<geometry_msgs::Point>& point_vector_right = marker_array.markers[1].points;
marker_array.markers[1].color.r = 0.0;
marker_array.markers[1].color.b = 1.0;
marker_array.markers[1].id = 1;
Eigen::Affine3d rot_left (Eigen::AngleAxisd(M_PI*0.5, Eigen::Vector3d::UnitX())*
Eigen::AngleAxisd(steer_angle_left, Eigen::Vector3d::UnitY()));
tf::quaternionEigenToMsg(Eigen::Quaterniond(rot_left.rotation()), marker_array_.markers[LEFT_FRONT_WHEEL].pose.orientation);
Eigen::Affine3d rot_right (Eigen::AngleAxisd(M_PI*0.5, Eigen::Vector3d::UnitX())*
Eigen::AngleAxisd(steer_angle_right, Eigen::Vector3d::UnitY()));
tf::quaternionEigenToMsg(Eigen::Quaterniond(rot_right.rotation()), marker_array_.markers[RIGHT_FRONT_WHEEL].pose.orientation);
//marker_array_.markers[LEFT_FRONT_WHEEL].pose.orientation; // = marker;
//marker_array_.markers[RIGHT_FRONT_WHEEL].pose.orientation; // = marker;
Eigen::Vector3d rotation_vector( Eigen::Vector3d::UnitZ() );
if (turn_radius > 0.0){
rotation_vector = -rotation_vector;
}
//std::cout << "rotation_vector:\n" << rotation_vector << "\n";
for (size_t i = 0; i < 40; ++i)
{
Eigen::Affine3d o_t_i (Eigen::Affine3d::Identity());
o_t_i.translation() = Eigen::Vector3d(icc.x(), -icc.y(), 0.0);
//Eigen::Rotation2Dd rotation(steer_angle_left + static_cast<double>(i) * 0.05);
Eigen::Affine3d rotation_left (Eigen::AngleAxisd(static_cast<double>(i) * 0.05,
rotation_vector));
//Eigen::Affine3d rotation_left (Eigen::AngleAxisd( static_cast<double>(i) * 0.05,
// (turn_radius > 0.0) ? -(Eigen::Vector3d::UnitZ()) : Eigen::Vector3d::UnitZ() ));
//Eigen::Vector2d tmp(o_t_i * rotation * left_wheel);
//Eigen::Vector2d tmp(o_t_i * rotation * left_wheel).translation();
Eigen::Vector3d tmp(o_t_i * rotation_left *Eigen::Vector3d(p_wheel_base_, (turn_radius)-p_wheel_track_*0.5, 0.0));
point_vector_left[i].x = tmp.x();
point_vector_left[i].y = -tmp.y();
Eigen::Affine3d rotation_right (Eigen::AngleAxisd(static_cast<double>(i) * 0.05,
rotation_vector));
//Eigen::Affine3d rotation_right (Eigen::AngleAxisd( static_cast<double>(i) * 0.05,
// (turn_radius > 0.0) ? -(Eigen::Vector3d::UnitZ()) : Eigen::Vector3d::UnitZ() ));
tmp = o_t_i * rotation_right*Eigen::Vector3d(p_wheel_base_, (turn_radius)+p_wheel_track_*0.5, 0.0);
point_vector_right[i].x = tmp.x();
point_vector_right[i].y = -tmp.y();
}
}
