diff --git a/robots/beetle/include/beetle/beetle_navigation.h b/robots/beetle/include/beetle/beetle_navigation.h index 9f2695294..776dea6e1 100644 --- a/robots/beetle/include/beetle/beetle_navigation.h +++ b/robots/beetle/include/beetle/beetle_navigation.h @@ -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 beetle_robot_model_; diff --git a/robots/beetle/src/beetle_navigation.cpp b/robots/beetle/src/beetle_navigation.cpp index ae7ae66db..f66bd180a 100644 --- a/robots/beetle/src/beetle_navigation.cpp +++ b/robots/beetle/src/beetle_navigation.cpp @@ -6,7 +6,8 @@ using namespace aerial_robot_model; using namespace aerial_robot_navigation; BeetleNavigator::BeetleNavigator(): - GimbalrotorNavigator() + GimbalrotorNavigator(), + roll_pitch_control_flag_(false) { } @@ -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; @@ -275,6 +557,13 @@ void BeetleNavigator::convertTargetPosFromCoG2CoM() pre_target_pos_.setZ(target_cog_pos.z()); } +void BeetleNavigator::rosParamInit() +{ + ros::NodeHandle nh(nh_, "navigation"); + getParam(nh, "max_target_roll_pitch_rate", max_target_roll_pitch_rate_, 0.0); + GimbalrotorNavigator::rosParamInit(); +} + /* plugin registration */ #include