Skip to content

Commit

Permalink
Minor changes in api.cpp
Browse files Browse the repository at this point in the history
  • Loading branch information
novakfi8 committed Nov 4, 2024
1 parent 6e577bb commit be3064b
Showing 1 changed file with 2 additions and 4 deletions.
6 changes: 2 additions & 4 deletions src/api.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -526,8 +526,6 @@ bool MrsUsvArduroverApi::callbackPositionCmd([[maybe_unused]] const mrs_msgs::Hw

ROS_INFO_ONCE("[%s]: getting position cmd", node_name.c_str());

ROS_INFO("[%s]: getting position cmd", node_name.c_str());

if (!_capabilities_.accepts_position_cmd) {
ROS_ERROR("[%s]: the position input is not enabled in the config file", node_name.c_str());
return false;
Expand Down Expand Up @@ -578,7 +576,6 @@ bool MrsUsvArduroverApi::callbackPositionCmd([[maybe_unused]] const mrs_msgs::Hw

// fill position target command
position_target.header.stamp = ros::Time::now();
position_target.coordinate_frame = position_target.FRAME_LOCAL_NED;

position_target.type_mask = 0;
position_target.type_mask += position_target.IGNORE_VX+position_target.IGNORE_VY+position_target.IGNORE_VZ;
Expand All @@ -590,7 +587,8 @@ bool MrsUsvArduroverApi::callbackPositionCmd([[maybe_unused]] const mrs_msgs::Hw
position_target.position.y = pose_transformed.value().pose.pose.position.y;
position_target.position.z = pose_transformed.value().pose.pose.position.z;
position_target.yaw = position_target_yaw;


// publish position target command
ph_mavros_position_target_.publish(position_target);

return true;
Expand Down

0 comments on commit be3064b

Please sign in to comment.