Skip to content

Commit

Permalink
fix servo camera plugin
Browse files Browse the repository at this point in the history
  • Loading branch information
@kratkvit
kratkvit committed Jan 30, 2024
1 parent 658d1cb commit 1f75a9f
Showing 1 changed file with 77 additions and 76 deletions.
153 changes: 77 additions & 76 deletions src/sensor_and_model_plugins/servo_camera_plugin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;

Expand All @@ -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<double>("max_pitch_rate");
Expand All @@ -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<double>("max_pitch");
} else {
Expand All @@ -133,16 +134,16 @@ 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<double>("max_roll");
} else {
ROS_WARN("[%s][Servo camera]: max_roll not defined. Setting to default value.", parent_name.c_str());
max_roll = 0.5;
}

if (_sdf->HasElement("tilt_update_rate")) {
update_rate = _sdf->Get<double>("tilt_update_rate");
if (_sdf->HasElement("update_rate")) {
update_rate = _sdf->Get<double>("update_rate");
} else {
ROS_WARN("[%s][Servo camera]: Update_rate not defined. Setting to default value.", parent_name.c_str());
update_rate = 30;
Expand Down Expand Up @@ -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;
Expand All @@ -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());
}
Expand All @@ -218,31 +219,30 @@ void ServoCameraPlugin::Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf) {
/* cameraTimerCallback */ //{
void ServoCameraPlugin::cameraTimerCallback(const ros::TimerEvent &event) {

if (!initialized) {
if (!initialized) {
return;
}

moveCamera();
}
//}

/* 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]);

}
//}

Expand All @@ -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;

}
//}

Expand All @@ -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;
}
//}

Expand All @@ -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
Expand All @@ -316,16 +321,15 @@ 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;
}
}

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());
}
Expand All @@ -344,16 +348,15 @@ 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;
}
}

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());
}
Expand All @@ -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();
}
//}

Expand Down

0 comments on commit 1f75a9f

Please sign in to comment.