diff --git a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h index b6ed1b343..9e9aca157 100644 --- a/aerial_robot_control/include/aerial_robot_control/flight_navigation.h +++ b/aerial_robot_control/include/aerial_robot_control/flight_navigation.h @@ -463,9 +463,6 @@ namespace aerial_robot_navigation if(!teleop_flag_) return; setNaviState(LAND_STATE); - - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); ROS_INFO("Land state"); } diff --git a/aerial_robot_control/src/flight_navigation.cpp b/aerial_robot_control/src/flight_navigation.cpp index 82c074528..c6cc14f1f 100644 --- a/aerial_robot_control/src/flight_navigation.cpp +++ b/aerial_robot_control/src/flight_navigation.cpp @@ -447,8 +447,6 @@ void BaseNavigator::joyStickControl(const sensor_msgs::JoyConstPtr & joy_msg) setNaviState(LAND_STATE); //update - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); ROS_INFO("Joy Control: Land state"); return; @@ -682,8 +680,6 @@ void BaseNavigator::update() if(normal_land && !force_att_control_flag_) { setNaviState(LAND_STATE); - setTargetXyFromCurrentState(); - setTargetYawFromCurrentState(); } } @@ -1054,9 +1050,20 @@ void BaseNavigator::rosParamInit() ros::NodeHandle nh(nh_, "navigation"); getParam(nh, "xy_control_mode", xy_control_mode_, 0); getParam(nh, "takeoff_height", takeoff_height_, 0.0); + getParam(nh, "land_descend_vel",land_descend_vel_, -0.3); + if (land_descend_vel_ >= 0) { + ROS_WARN("land_descend_vel_ (current value: %f) should be negative", land_descend_vel_); + land_descend_vel_ == -0.3; + } + getParam(nh, "hover_convergent_duration", hover_convergent_duration_, 1.0); getParam(nh, "land_check_duration", land_check_duration_, 0.5); + if (land_check_duration_ < 0.5) { + ROS_WARN("land_check_duration_ (current value: %f) should be not smaller than 0.5", land_check_duration_); + land_check_duration_ = 0.5; + } + getParam(nh, "trajectory_reset_duration", trajectory_reset_duration_, 0.5); getParam(nh, "teleop_reset_duration", teleop_reset_duration_, 0.5); getParam(nh, "z_convergent_thresh", z_convergent_thresh_, 0.05);