Skip to content

Commit

Permalink
[Ninja][Navigation] Workaround to impliment new velocity control fram…
Browse files Browse the repository at this point in the history 
…e: LEFT_DOCK and RIGHT_DOCK
  • Loading branch information
@sugihara-16
sugihara-16 committed Dec 27, 2024
1 parent be0b25c commit a4f3e38
Show file tree
Hide file tree
Showing 3 changed files with 55 additions and 45 deletions.
2 changes: 2 additions & 0 deletions aerial_robot_msgs/msg/FlightNav.msg
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,8 @@ uint8 STAY_HERE_MODE = 6

uint8 WORLD_FRAME = 0
uint8 LOCAL_FRAME = 1
uint8 LEFT_DOCK = 2
uint8 RIGHT_DOCK = 3

uint8 BASELINK = 0
uint8 COG = 1
Expand Down
8 changes: 7 additions & 1 deletion robots/ninja/include/ninja/ninja_navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -22,7 +22,13 @@ namespace aerial_robot_navigation
FRAC,
EXP
};


enum ninja_control_frame
{
LEFT_DOCK = LOCAL_FRAME + 1,
RIGHT_DOCK
};

class ModuleData
{
public:
Expand Down
90 changes: 46 additions & 44 deletions robots/ninja/src/ninja_navigation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -846,50 +846,52 @@ void NinjaNavigator::assemblyNavCallback(const aerial_robot_msgs::FlightNavConst
setTargetVelCandY(target_vel.y());
break;
}
// case LEFT_DOCK:
// {
// try
// {
// tfBuffer_.canTransform("world", my_name_ + std::to_string(left_id_) + std::string("/pitch_connect_point"), ros::Time::now(), ros::Duration(0.1));
// KDL::Frame current_com;
// geometry_msgs::TransformStamped transformStamped;
// transformStamped = tfBuffer_.lookupTransform("world", my_name_ + std::to_string(left_id_) + std::string("/pitch_connect_point") , ros::Time(0));
// tf::transformMsgToKDL(transformStamped.transform, current_com);

// current_com.M.GetEulerZYX(current_com_yaw, current_com_pitch, current_com_roll);
// }
// catch (tf2::TransformException& ex)
// {
// ROS_ERROR_STREAM("CoM is not defined");
// return;
// }
// tf::Vector3 target_vel = frameConversion(tf::Vector3(msg->target_vel_x, msg->target_vel_y, 0), current_com_yaw);
// setTargetVelCandX(target_vel.x());
// setTargetVelCandY(target_vel.y());
// break;
// }
// case RIGHT_DOCK:
// {
// try
// {
// tfBuffer_.canTransform("world", my_name_ + std::to_string(right_id_) + std::string("/yaw_connect_point"), ros::Time::now(), ros::Duration(0.1));
// KDL::Frame current_com;
// geometry_msgs::TransformStamped transformStamped;
// transformStamped = tfBuffer_.lookupTransform("world", my_name_ + std::to_string(right_id_) + std::string("/yaw_connect_point") , ros::Time(0));
// tf::transformMsgToKDL(transformStamped.transform, current_com);

// current_com.M.GetEulerZYX(current_com_yaw, current_com_pitch, current_com_roll);
// }
// catch (tf2::TransformException& ex)
// {
// ROS_ERROR_STREAM("CoM is not defined");
// return;
// }
// tf::Vector3 target_vel = frameConversion(tf::Vector3(msg->target_vel_x, msg->target_vel_y, 0), current_com_yaw);
// setTargetVelCandX(target_vel.x());
// setTargetVelCandY(target_vel.y());
// break;
// }
case LEFT_DOCK:
{
double current_com_roll, current_com_pitch, current_com_yaw;
try
{
tfBuffer_.canTransform("world", my_name_ + std::to_string(left_id_) + std::string("/pitch_connect_point"), ros::Time::now(), ros::Duration(0.1));
KDL::Frame current_left_dock;
geometry_msgs::TransformStamped transformStamped;
transformStamped = tfBuffer_.lookupTransform("world", my_name_ + std::to_string(left_id_) + std::string("/pitch_connect_point") , ros::Time(0));
tf::transformMsgToKDL(transformStamped.transform, current_left_dock);

current_left_dock.M.GetEulerZYX(current_com_yaw, current_com_pitch, current_com_roll);
}
catch (tf2::TransformException& ex)
{
ROS_ERROR_STREAM("CoM is not defined");
return;
}
tf::Vector3 target_vel = frameConversion(tf::Vector3(msg->target_vel_x, msg->target_vel_y, 0), current_com_yaw);
setTargetVelCandX(target_vel.x());
setTargetVelCandY(target_vel.y());
break;
}
case RIGHT_DOCK:
{
double current_com_roll, current_com_pitch, current_com_yaw;
try
{
tfBuffer_.canTransform("world", my_name_ + std::to_string(right_id_) + std::string("/yaw_connect_point"), ros::Time::now(), ros::Duration(0.1));
KDL::Frame current_right_dock;
geometry_msgs::TransformStamped transformStamped;
transformStamped = tfBuffer_.lookupTransform("world", my_name_ + std::to_string(right_id_) + std::string("/yaw_connect_point") , ros::Time(0));
tf::transformMsgToKDL(transformStamped.transform, current_right_dock);

current_right_dock.M.GetEulerZYX(current_com_yaw, current_com_pitch, current_com_roll);
}
catch (tf2::TransformException& ex)
{
ROS_ERROR_STREAM("CoM is not defined");
return;
}
tf::Vector3 target_vel = frameConversion(tf::Vector3(msg->target_vel_x, msg->target_vel_y, 0), current_com_yaw);
setTargetVelCandX(target_vel.x());
setTargetVelCandY(target_vel.y());
break;
}
default:
{
break;
Expand Down

0 comments on commit a4f3e38

Please sign in to comment.