diff --git a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h index 7778cdc9..99753ee3 100644 --- a/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h +++ b/rm_orientation_controller/include/rm_orientation_controller/orientation_controller.h @@ -38,7 +38,7 @@ class Controller : public controller_interface::MultiInterfaceController 0.1 || abs(cal_pitch) > 0.1 ) + if (abs(cal_roll) > 0.1 || abs(cal_pitch) > 0.1 ) { init_calibration = true; - ROS_INFO_THROTTLE(0.1, "Forced calibration success"); + ROS_INFO_THROTTLE(0.1, "Forced calibrate success"); } - if(getCalTimes > 100) + if (getCalTimes > 30) { forced_calibration = false; - ROS_INFO_THROTTLE(0.1, "Forced calibration failed"); + ROS_INFO_THROTTLE(0.1, "Forced calibrate failed"); } } catch (tf2::TransformException& ex) @@ -79,7 +80,6 @@ void Controller::update(const ros::Time& time, const ros::Duration& period) ROS_WARN("%s", ex.what()); } } - } } @@ -107,12 +107,12 @@ bool Controller::getTransform(const ros::Time& time, geometry_msgs::TransformSta } tf2::Quaternion odom2fixed_quat; odom2fixed_quat.setValue(x, y, z, w); - if( forced_calibration && init_calibration) + if (forced_calibration && init_calibration) { double ori_roll, ori_pitch, ori_yaw; tf2::Matrix3x3 m1(odom2fixed_quat); m1.getRPY(ori_roll, ori_pitch, ori_yaw); - odom2fixed_quat.setRPY(ori_roll+cal_roll, ori_pitch+cal_pitch, ori_yaw); + odom2fixed_quat.setRPY(ori_roll + cal_roll, ori_pitch + cal_pitch, ori_yaw); odom2fixed_quat.normalize(); } odom2fixed.setRotation(odom2fixed_quat); @@ -121,7 +121,6 @@ bool Controller::getTransform(const ros::Time& time, geometry_msgs::TransformSta return true; } -\ void Controller::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) { if (!receive_imu_msg_) @@ -133,7 +132,6 @@ void Controller::imuDataCallback(const sensor_msgs::Imu::ConstPtr& msg) tf_broadcaster_.sendTransform(source2target); } - } // namespace rm_orientation_controller PLUGINLIB_EXPORT_CLASS(rm_orientation_controller::Controller, controller_interface::ControllerBase)