diff --git a/src/sensor_and_model_plugins/servo_camera_plugin.cpp b/src/sensor_and_model_plugins/servo_camera_plugin.cpp index 348a32e..9772160 100644 --- a/src/sensor_and_model_plugins/servo_camera_plugin.cpp +++ b/src/sensor_and_model_plugins/servo_camera_plugin.cpp @@ -48,42 +48,43 @@ class ServoCameraPlugin : public ModelPlugin { bool triggerTiltCompensationPitchCallback(std_srvs::SetBoolRequest &req, std_srvs::SetBoolResponse &res); bool triggerTiltCompensationRollCallback(std_srvs::SetBoolRequest &req, std_srvs::SetBoolResponse &res); - void updateModelOrientation(); - void OnUpdate(); - void cameraTimerCallback(const ros::TimerEvent &event); - void moveCamera(); + void updateModelOrientation(); + void OnUpdate(); + void cameraTimerCallback(const ros::TimerEvent &event); + void moveCamera(); + double getBoundedAngle(double angle); // camera and gimbal link and joint names - std::string parent_name; - std::string joint_name_pitch; - std::string joint_name_roll; - std::string parent_link_pitch; - std::string parent_link_roll; + std::string parent_name; + std::string joint_name_pitch; + std::string joint_name_roll; + std::string parent_link_pitch; + std::string parent_link_roll; // camera offset with respect to model - double offset_roll = 0.0; - double offset_pitch = 0.0; + double offset_roll = 0.0; + double offset_pitch = 0.0; - double update_rate; // update rate of camera position + double update_rate; // update rate of camera position - double desired_pitch; - double min_pitch; - double max_pitch; - double max_pitch_rate; - bool compensate_tilt_pitch; + double desired_pitch; + double min_pitch; + double max_pitch; + double max_pitch_rate; + bool compensate_tilt_pitch; - double desired_roll; - double min_roll; - double max_roll; - double max_roll_rate; - bool compensate_tilt_roll; + double desired_roll; + double min_roll; + double max_roll; + double max_roll_rate; + bool compensate_tilt_roll; - std::mutex mutex_desired_orientation; + std::mutex mutex_desired_orientation; // model orientation in world frame - std::mutex mutex_model_orientation; - double model_roll; - double model_pitch; + std::mutex mutex_model_orientation; + double model_roll; + double model_pitch; bool initialized = false; @@ -94,10 +95,10 @@ class ServoCameraPlugin : public ModelPlugin { /* Load */ //{ void ServoCameraPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { ROS_INFO("[%s]: Servo camera plugin started.", ros::this_node::getName().c_str()); - model = _parent; - parent_name = model->GetName().c_str(); + model = _parent; + parent_name = model->GetName().c_str(); -/* Load params //{ */ + /* Load params //{ */ if (_sdf->HasElement("max_pitch_rate")) { max_pitch_rate = _sdf->Get("max_pitch_rate"); @@ -119,7 +120,7 @@ void ServoCameraPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { ROS_WARN("[%s][Servo camera]: min_pitch not defined. Setting to default value.", parent_name.c_str()); min_pitch = -1.57; } - + if (_sdf->HasElement("max_pitch")) { max_pitch = _sdf->Get("max_pitch"); } else { @@ -133,7 +134,7 @@ void ServoCameraPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { ROS_WARN("[%s][Servo camera]: min_roll not defined. Setting to default value.", parent_name.c_str()); min_roll = -0.5; } - + if (_sdf->HasElement("max_roll")) { max_roll = _sdf->Get("max_roll"); } else { @@ -141,8 +142,8 @@ void ServoCameraPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { max_roll = 0.5; } - if (_sdf->HasElement("tilt_update_rate")) { - update_rate = _sdf->Get("tilt_update_rate"); + if (_sdf->HasElement("update_rate")) { + update_rate = _sdf->Get("update_rate"); } else { ROS_WARN("[%s][Servo camera]: Update_rate not defined. Setting to default value.", parent_name.c_str()); update_rate = 30; @@ -183,7 +184,7 @@ void ServoCameraPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { } else { ROS_WARN("[%s][Servo camera]: Tilt roll compensation not defined.", parent_name.c_str()); } -//} + //} // initialize ROS int argc = 0; @@ -203,13 +204,13 @@ void ServoCameraPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { ss.str(std::string()); ss << "/" << parent_name << "/servo_camera/compensate_tilt_roll"; - tilt_compensation_roll_srv = nh->advertiseService(ss.str().c_str(), &ServoCameraPlugin::triggerTiltCompensationPitchCallback, this); + tilt_compensation_roll_srv = nh->advertiseService(ss.str().c_str(), &ServoCameraPlugin::triggerTiltCompensationRollCallback, this); camera_timer = nh->createTimer(ros::Rate(update_rate), &ServoCameraPlugin::cameraTimerCallback, this); updateConnection = event::Events::ConnectWorldUpdateBegin(std::bind(&ServoCameraPlugin::OnUpdate, this)); - initialized = true; + initialized = true; ROS_INFO("[%s][Servo camera]: Servo camera initialized.", parent_name.c_str()); } @@ -218,7 +219,7 @@ void ServoCameraPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) { /* cameraTimerCallback */ //{ void ServoCameraPlugin::cameraTimerCallback(const ros::TimerEvent &event) { - if (!initialized) { + if (!initialized) { return; } @@ -226,23 +227,22 @@ void ServoCameraPlugin::cameraTimerCallback(const ros::TimerEvent &event) { } //} -/* pitchCallback */ //{ +/* desiredOrientationCallback */ //{ void ServoCameraPlugin::desiredOrientationCallback(const std_msgs::Float32MultiArray &msg) { - if (msg.data.size() != 2) { + if (msg.data.size() != 2) { ROS_INFO("[%s][Servo camera]: cameraOrientationCallback: Invalid request. Unexpected size of data.", parent_name.c_str()); return; } std::scoped_lock lock(mutex_desired_orientation); - desired_pitch = fmin(fmax(min_pitch, msg.data[1]), max_pitch); // clip value in limits - desired_pitch = fmod(desired_pitch + 2 * M_PI, 2 * M_PI); + desired_pitch = fmin(fmax(min_pitch, msg.data[1]), max_pitch); // clip value in limits + desired_pitch = getBoundedAngle(desired_pitch); ROS_INFO("[%s][Servo camera]: Change camera pitch to %.2f! Requested unlimited pitch = %.2f", parent_name.c_str(), desired_pitch, msg.data[1]); - desired_roll = fmin(fmax(min_roll, msg.data[0]), max_roll); // clip value in limits - desired_roll = fmod(desired_roll + 2 * M_PI, 2 * M_PI); + desired_roll = fmin(fmax(min_roll, msg.data[0]), max_roll); // clip value in limits + desired_roll = getBoundedAngle(desired_roll); ROS_INFO("[%s][Servo camera]: Change camera roll to %.2f! Requested unlimited roll = %.2f", parent_name.c_str(), desired_pitch, msg.data[0]); - } //} @@ -252,12 +252,11 @@ bool ServoCameraPlugin::triggerTiltCompensationPitchCallback(std_srvs::SetBoolRe ROS_INFO("[%s][Servo camera]: Trigger tilt compensation pitch!", parent_name.c_str()); compensate_tilt_pitch = req.data; - res.message = compensate_tilt_pitch ? "tilt compensation enabled" : "tilt compensation disabled"; + res.message = compensate_tilt_pitch ? "tilt compensation enabled" : "tilt compensation disabled"; ROS_WARN("[%s][Servo Camera]: %s.", parent_name.c_str(), res.message.c_str()); res.success = true; return res.success; - } //} @@ -267,12 +266,17 @@ bool ServoCameraPlugin::triggerTiltCompensationRollCallback(std_srvs::SetBoolReq ROS_INFO("[%s][Servo camera]: Trigger tilt compensation roll!", parent_name.c_str()); compensate_tilt_roll = req.data; - res.message = compensate_tilt_roll ? "tilt compensation enabled" : "tilt compensation disabled"; + res.message = compensate_tilt_roll ? "tilt compensation enabled" : "tilt compensation disabled"; ROS_WARN("[%s][Servo Camera]: %s.", parent_name.c_str(), res.message.c_str()); res.success = true; return res.success; +} +//} +/* getBoundedAngle(double angle) //{ */ +double ServoCameraPlugin::getBoundedAngle(double angle) { + return angle > M_PI ? -2 * M_PI + angle : angle < -M_PI ? 2 * M_PI + angle : angle; } //} @@ -283,30 +287,31 @@ void ServoCameraPlugin::moveCamera() { std::scoped_lock lock(mutex_desired_orientation, mutex_model_orientation); // get desired pitch and roll angles compensated for current tilts of model - double compensated_pitch = compensate_tilt_pitch ? desired_pitch - model_pitch : desired_pitch; - double compensated_roll = compensate_tilt_roll ? desired_roll - model_roll : desired_roll; + double compensated_pitch = compensate_tilt_pitch ? desired_pitch - model_pitch : desired_pitch; + double compensated_roll = compensate_tilt_roll ? desired_roll - model_roll : desired_roll; + + // find required angle in next step that satisfies limits on angular_rates + double abs_diff_pitch = abs(offset_pitch - compensated_pitch); + double min_diff_pitch = abs_diff_pitch > M_PI ? 2 * M_PI - abs_diff_pitch : abs_diff_pitch; - // find required angle in next step that satisfies limits on angular_rates - double abs_diff_pitch = abs(offset_pitch - compensated_pitch); - double min_diff_pitch = abs_diff_pitch > M_PI ? 2 * M_PI - abs_diff_pitch : abs_diff_pitch; + if (abs(min_diff_pitch) > 1e-4) { - if (min_diff_pitch > 1e-2) { double dir = compensated_pitch > offset_pitch ? 1.0 : -1.0; - dir = abs(min_diff_pitch - abs_diff_pitch) > 1e-2 ? -1.0 * dir : dir; - offset_pitch += dir * fmin(abs(compensated_pitch - offset_pitch), max_pitch_rate / update_rate); - offset_pitch = fmod(offset_pitch + 2 * M_PI, 2 * M_PI); + dir = abs(min_diff_pitch - abs_diff_pitch) > 1e-4 ? -1.0 * dir : dir; + offset_pitch += dir * fmin(abs(min_diff_pitch), max_pitch_rate / update_rate); + offset_pitch = getBoundedAngle(offset_pitch); } - double abs_diff_roll = abs(offset_roll - compensated_roll); - double min_diff_roll = abs_diff_roll > M_PI ? 2 * M_PI - abs_diff_roll : abs_diff_roll; + double abs_diff_roll = abs(offset_roll - compensated_roll); + double min_diff_roll = abs_diff_roll > M_PI ? 2 * M_PI - abs_diff_roll : abs_diff_roll; + + if (abs(min_diff_roll) > 1e-4) { - if (min_diff_roll > 1e-2) { double dir = compensated_roll > offset_roll ? 1.0 : -1.0; - dir = abs(min_diff_roll - abs_diff_roll) > 1e-2 ? -1.0 * dir : dir; - offset_roll += dir * fmin(abs(compensated_roll - offset_roll), max_roll_rate / update_rate); - offset_roll = fmod(offset_roll + 2 * M_PI, 2 * M_PI); + dir = abs(min_diff_roll - abs_diff_roll) > 1e-4 ? -1.0 * dir : dir; + offset_roll += dir * fmin(abs(min_diff_roll), max_roll_rate / update_rate); + offset_roll = getBoundedAngle(offset_roll); } - } // set joint coordinates based on computed required orientations of links @@ -316,6 +321,7 @@ void ServoCameraPlugin::moveCamera() { int index_roll = -1; for (size_t i = 0; i < model->GetLink(parent_link_roll)->GetChildJoints().size(); i++) { + if (model->GetLink(parent_link_roll)->GetChildJoints()[i]->GetName() == joint_name_roll) { index_roll = i; break; @@ -323,9 +329,7 @@ void ServoCameraPlugin::moveCamera() { } if (index_roll != -1) { - double applied_roll_offset = offset_roll > M_PI ? -2*M_PI + offset_roll : offset_roll; - model->GetLink(parent_link_roll)->GetChildJoints()[index_roll]->SetPosition(0, applied_roll_offset); - ROS_INFO_THROTTLE(5.0, "[%s]: Setting position of joint %s to %.2f.", ros::this_node::getName().c_str(), model->GetLink(parent_link_roll)->GetChildJoints()[index_roll]->GetName().c_str(), applied_roll_offset); + model->GetLink(parent_link_roll)->GetChildJoints()[index_roll]->SetPosition(0, offset_roll); } else { ROS_WARN_THROTTLE(1.0, "[%s]: Servo camera roll joint did not found.", ros::this_node::getName().c_str()); } @@ -344,6 +348,7 @@ void ServoCameraPlugin::moveCamera() { int index_pitch = -1; for (size_t i = 0; i < model->GetLink(parent_link_pitch)->GetChildJoints().size(); i++) { + if (model->GetLink(parent_link_pitch)->GetChildJoints()[i]->GetName() == joint_name_pitch) { index_pitch = i; break; @@ -351,9 +356,7 @@ void ServoCameraPlugin::moveCamera() { } if (index_pitch != -1) { - double applied_pitch_offset = offset_pitch > M_PI ? -2*M_PI + offset_pitch : offset_pitch; - model->GetLink(parent_link_pitch)->GetChildJoints()[index_pitch]->SetPosition(0, applied_pitch_offset); - ROS_INFO_THROTTLE(5.0, "[%s]: Setting position of joint %s to %.2f.", ros::this_node::getName().c_str(), model->GetLink(parent_link_pitch)->GetChildJoints()[index_pitch]->GetName().c_str(), applied_pitch_offset); + model->GetLink(parent_link_pitch)->GetChildJoints()[index_pitch]->SetPosition(0, offset_pitch); } else { ROS_WARN_THROTTLE(1.0, "[%s]: Servo camera pitch joint did not found.", ros::this_node::getName().c_str()); } @@ -365,20 +368,18 @@ void ServoCameraPlugin::moveCamera() { } else { ROS_WARN_THROTTLE(1.0, "[%s]: Model has no link %s", ros::this_node::getName().c_str(), parent_link_pitch.c_str()); } - } //} /* updateCameraPosition() //{ */ void ServoCameraPlugin::updateModelOrientation() { - + // get tilts of model - std::scoped_lock lock(mutex_model_orientation); + ROS_INFO_ONCE("[%s]: Updating model orientation.", ros::this_node::getName().c_str()); + std::scoped_lock lock(mutex_model_orientation); ignition::math::Pose3d model_pose = model->WorldPose(); - model_roll = model_pose.Rot().Roll(); - model_pitch = model_pose.Rot().Pitch(); - ROS_INFO_THROTTLE(5.0, "[%s]: Updating model orientation.", ros::this_node::getName().c_str()); - + model_roll = model_pose.Rot().Roll(); + model_pitch = model_pose.Rot().Pitch(); } //}