Skip to content

Commit

Permalink
[Beetle][Navigation] add joy command to control roll pitch angles
Browse files Browse the repository at this point in the history
  • Loading branch information
@sugihara-16
sugihara-16 committed Sep 16, 2023
1 parent cf91a1b commit e4c6157
Show file tree
Hide file tree
Showing 2 changed files with 299 additions and 1 deletion.
9 changes: 9 additions & 0 deletions robots/beetle/include/beetle/beetle_navigation.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -34,12 +34,21 @@ namespace aerial_robot_navigation
inline void setTargetPosCand( tf::Vector3 value){ target_pos_candidate_ = value ;}
inline tf::Vector3 getTargetPosCand() {return target_pos_candidate_;}

protected:
void rosParamInit() override;

private:
tf::Vector3 target_pos_candidate_;
tf::Vector3 pre_target_pos_;
ros::NodeHandle nh_;
ros::NodeHandle nhp_;

double max_target_roll_pitch_rate_;

bool roll_pitch_control_flag_;

void naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg) override;
void joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) override;
void rotateContactPointFrame();
tf2_ros::TransformBroadcaster br_;
boost::shared_ptr<BeetleRobotModel> beetle_robot_model_;
Expand Down
291 changes: 290 additions & 1 deletion robots/beetle/src/beetle_navigation.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,7 +6,8 @@ using namespace aerial_robot_model;
using namespace aerial_robot_navigation;

BeetleNavigator::BeetleNavigator():
GimbalrotorNavigator()
GimbalrotorNavigator(),
roll_pitch_control_flag_(false)
{
}

Expand All @@ -25,6 +26,287 @@ void BeetleNavigator::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
}

}

void BeetleNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg)
{
sensor_msgs::Joy joy_cmd;
if(joy_msg->axes.size() == PS3_AXES && joy_msg->buttons.size() == PS3_BUTTONS)
{
joy_cmd = (*joy_msg);
}
else if(joy_msg->axes.size() == PS4_AXES && joy_msg->buttons.size() == PS4_BUTTONS)
{
joy_cmd = ps4joyToPs3joyConvert(*joy_msg);
}
else
{
ROS_WARN("the joystick type is not supported (buttons: %d, axes: %d)", (int)joy_msg->buttons.size(), (int)joy_msg->axes.size());
return;
}

/* ps3 joy bottons assignment: http://wiki.ros.org/ps3joy */
if(!joy_stick_heart_beat_) joy_stick_heart_beat_ = true;
joy_stick_prev_time_ = ros::Time::now().toSec();

/* common command */
/* start */
if(joy_cmd.buttons[PS3_BUTTON_START] == 1 && getNaviState() == ARM_OFF_STATE)
{
motorArming();
return;
}

/* force landing && halt */
if(joy_cmd.buttons[PS3_BUTTON_SELECT] == 1)
{
/* Force Landing in inflight mode: TAKEOFF_STATE/LAND_STATE/HOVER_STATE */
if(!force_landing_flag_ && (getNaviState() == TAKEOFF_STATE || getNaviState() == LAND_STATE || getNaviState() == HOVER_STATE))
{
ROS_WARN("Joy Control: force landing state");
spinal::FlightConfigCmd flight_config_cmd;
flight_config_cmd.cmd = spinal::FlightConfigCmd::FORCE_LANDING_CMD;
flight_config_pub_.publish(flight_config_cmd);
force_landing_flag_ = true;

/* update the force landing stamp for the halt process*/
force_landing_start_time_ = joy_cmd.header.stamp;
}

/* Halt mode */
if(joy_cmd.header.stamp.toSec() - force_landing_start_time_.toSec() > force_landing_to_halt_du_ && getNaviState() > START_STATE)
{
//if(!teleop_flag_) return; /* can not do the process if other processs are running */

ROS_ERROR("Joy Control: Halt!");

setNaviState(STOP_STATE);

/* update the target pos(maybe not necessary) */
setTargetXyFromCurrentState();
setTargetYawFromCurrentState();
}
return;
}
else
{
/* update the halt process */
force_landing_start_time_ = joy_cmd.header.stamp;
}

/* takeoff */
if(joy_cmd.buttons[PS3_BUTTON_CROSS_LEFT] == 1 && joy_cmd.buttons[PS3_BUTTON_ACTION_CIRCLE] == 1)
{
startTakeoff();
return;
}

/* landing */
if(joy_cmd.buttons[PS3_BUTTON_CROSS_RIGHT] == 1 && joy_cmd.buttons[PS3_BUTTON_ACTION_SQUARE] == 1)
{
if(force_att_control_flag_) return;

if(getNaviState() == LAND_STATE) return;
if(!teleop_flag_) return; /* can not do the process if other processs are running */

setNaviState(LAND_STATE);
//update
setTargetXyFromCurrentState();
setTargetYawFromCurrentState();
setTargetPosZ(estimator_->getLandingHeight());
ROS_INFO("Joy Control: Land state");

return;
}

/* Motion: Up/Down */
if(fabs(joy_cmd.axes[PS3_AXIS_STICK_RIGHT_UPWARDS]) > joy_z_deadzone_)
{
if(getNaviState() == HOVER_STATE)
{
z_control_flag_ = true;
if(joy_cmd.axes[PS3_AXIS_STICK_RIGHT_UPWARDS] >= 0)
addTargetPosZ(joy_target_z_interval_);
else
addTargetPosZ(-joy_target_z_interval_);
}
}
else
{
if(z_control_flag_)
{
z_control_flag_= false;
setTargetZFromCurrentState();
ROS_INFO("Joy Control: fixed z state, target pos z is %f",target_pos_.z());
}
}

/* Motion: Yaw */
/* this is the yaw_angle control */
if(fabs(joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS]) > joy_yaw_deadzone_)
{
double target_yaw = estimator_->getState(State::YAW_COG, estimate_mode_)[0]
+ joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS] * max_target_yaw_rate_;
setTargetYaw(angles::normalize_angle(target_yaw));
setTargetOmageZ(joy_cmd.axes[PS3_AXIS_STICK_RIGHT_LEFTWARDS] * max_target_yaw_rate_);

yaw_control_flag_ = true;
}
else
{
if(yaw_control_flag_)
{
yaw_control_flag_= false;
setTargetYawFromCurrentState();
setTargetOmageZ(0);
ROS_INFO("Joy Control: fixed yaw state, target yaw angle is %f", getTargetRPY().z());
}
}

/* Motion: Roll and Pitch*/
/* this is the roll and pitch_angle control */
if(joy_cmd.buttons[PS3_BUTTON_REAR_LEFT_1] == 1)
{
tf::Vector3 target_roll_pitch;
target_roll_pitch.setX(getFinalTargetBaselinkRot().x()
+ joy_cmd.axes[PS3_AXIS_STICK_LEFT_LEFTWARDS] * max_target_roll_pitch_rate_
);


target_roll_pitch.setY(getFinalTargetBaselinkRot().y()
+ joy_cmd.axes[PS3_AXIS_STICK_LEFT_UPWARDS] * max_target_roll_pitch_rate_
);

setFinalTargetBaselinkRot(target_roll_pitch);
roll_pitch_control_flag_ = true;
}
else
{
if(roll_pitch_control_flag_)
{
roll_pitch_control_flag_= false;
ROS_INFO("Joy Control: fixed roll pich state, target roll angle is %f, pitch angle is %f", getFinalTargetBaselinkRot().x(),getFinalTargetBaselinkRot().y());
}
}
if(joy_cmd.buttons[PS3_BUTTON_REAR_LEFT_1] == 1 && joy_cmd.buttons[PS3_BUTTON_REAR_RIGHT_1] == 1)
{
tf::Vector3 target_roll_pitch;
target_roll_pitch.setX(0.0);
target_roll_pitch.setY(0.0);
setFinalTargetBaselinkRot(target_roll_pitch);
}

/* turn to ACC_CONTROL_MODE */
if(joy_cmd.buttons[PS3_BUTTON_CROSS_DOWN] == 1)
{
ROS_WARN("Foce change to attitude control");
force_att_control_flag_ = true;
estimator_->setForceAttControlFlag(force_att_control_flag_);
xy_control_mode_ = ACC_CONTROL_MODE;
}

/* change to vel control mode */
if(joy_cmd.buttons[PS3_BUTTON_ACTION_TRIANGLE] == 1 && !vel_control_flag_)
{
ROS_INFO("change to vel pos-based control");
vel_control_flag_ = true;
force_att_control_flag_ = false;
xy_control_mode_ = VEL_CONTROL_MODE;
target_vel_.setValue(0, 0, 0);
target_acc_.setValue(0, 0, 0);
}
if(joy_cmd.buttons[PS3_BUTTON_ACTION_TRIANGLE] == 0 && vel_control_flag_)
vel_control_flag_ = false;

/* change to pos control mode */
if(joy_cmd.buttons[PS3_BUTTON_ACTION_CROSS] == 1 && !pos_control_flag_)
{
ROS_INFO("change to pos control");
pos_control_flag_ = true;
force_att_control_flag_ = false;
xy_control_mode_ = POS_CONTROL_MODE;
setTargetXyFromCurrentState();
target_acc_.setValue(0, 0, 0);
}
if(joy_cmd.buttons[PS3_BUTTON_ACTION_CROSS] == 0 && pos_control_flag_)
pos_control_flag_ = false;

/* mode oriented state */
switch (xy_control_mode_)
{
case ACC_CONTROL_MODE:
{
if(teleop_flag_)
{
control_frame_ = WORLD_FRAME;
if(joy_cmd.buttons[PS3_BUTTON_REAR_LEFT_2]) control_frame_ = LOCAL_FRAME;

/* acc command */
target_acc_.setValue(joy_cmd.axes[PS3_AXIS_STICK_LEFT_UPWARDS] * max_target_tilt_angle_ * aerial_robot_estimation::G,
joy_cmd.axes[PS3_AXIS_STICK_LEFT_LEFTWARDS] * max_target_tilt_angle_ * aerial_robot_estimation::G, 0);

if(control_frame_ == LOCAL_FRAME)
{
tf::Vector3 target_acc = target_acc_;
/* convert the frame */
const auto segments_tf = robot_model_->getSegmentsTf();
if(segments_tf.find(teleop_local_frame_) == segments_tf.end())
{
ROS_ERROR("can not find %s in kinematics model", teleop_local_frame_.c_str());
target_acc.setValue(0,0,0);
}
tf::Transform teleop_local_frame_tf;
tf::transformKDLToTF(segments_tf.at(robot_model_->getBaselinkName()).Inverse() * segments_tf.at(teleop_local_frame_), teleop_local_frame_tf);

target_acc_ = frameConversion(target_acc, tf::Matrix3x3(tf::createQuaternionFromYaw(estimator_->getState(State::YAW_COG, estimate_mode_)[0])) * teleop_local_frame_tf.getBasis());
}
}
break;
}
case VEL_CONTROL_MODE:
{
/* during roll pitch rotaion, vel conrol is not available*/
if(joy_cmd.buttons[PS3_BUTTON_REAR_LEFT_1] == 1) break;

if(teleop_flag_)
{
control_frame_ = WORLD_FRAME;
if(joy_cmd.buttons[PS3_BUTTON_REAR_LEFT_2]) control_frame_ = LOCAL_FRAME;

tf::Vector3 target_vel(joy_cmd.axes[PS3_AXIS_STICK_LEFT_UPWARDS] * fabs(joy_cmd.axes[PS3_AXIS_STICK_LEFT_UPWARDS]) * max_target_vel_,
joy_cmd.axes[PS3_AXIS_STICK_LEFT_LEFTWARDS] * fabs(joy_cmd.axes[PS3_AXIS_STICK_LEFT_LEFTWARDS]) * max_target_vel_, 0);

/* defualt: world frame control */
/* L2 trigger: fc(cog/ baselink frame) frame control */
if(control_frame_ == LOCAL_FRAME)
{
tf::Vector3 target_vel_tmp = target_vel;
target_vel = frameConversion(target_vel_tmp, estimator_->getState(State::YAW_COG, estimate_mode_)[0]);
}

/* interpolation for vel target */
if(target_vel.x() - target_vel_.x() > joy_target_vel_interval_)
target_vel_ += tf::Vector3(joy_target_vel_interval_, 0, 0);
else if (target_vel.x() - target_vel_.x() < - joy_target_vel_interval_)
target_vel_ -= tf::Vector3(joy_target_vel_interval_, 0, 0);
else
target_vel_.setX(target_vel.x());

if(target_vel.y() - target_vel_.y() > joy_target_vel_interval_)
target_vel_ += tf::Vector3(0, joy_target_vel_interval_, 0);
else if (target_vel.y() - target_vel_.y() < - joy_target_vel_interval_)
target_vel_ -= tf::Vector3(0, joy_target_vel_interval_, 0);
else
target_vel_.setY(target_vel.y());
}
break;
}
default:
{
break;
}
}
}

void BeetleNavigator::naviCallback(const aerial_robot_msgs::FlightNavConstPtr & msg)
{ if(getNaviState() == TAKEOFF_STATE || BaseNavigator::getNaviState() == LAND_STATE) return;

Expand Down Expand Up @@ -275,6 +557,13 @@ void BeetleNavigator::convertTargetPosFromCoG2CoM()
pre_target_pos_.setZ(target_cog_pos.z());
}

void BeetleNavigator::rosParamInit()
{
ros::NodeHandle nh(nh_, "navigation");
getParam<double>(nh, "max_target_roll_pitch_rate", max_target_roll_pitch_rate_, 0.0);
GimbalrotorNavigator::rosParamInit();
}


/* plugin registration */
#include <pluginlib/class_list_macros.h>
Expand Down

0 comments on commit e4c6157

Please sign in to comment.