Skip to content

simplify twist conversion #44

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
jorgenfj wants to merge 1 commit into
base: main
Choose a base branch
from
+31 −11
Open

simplify twist conversion #44

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
42 changes: 31 additions & 11 deletions vortex_utils_ros/include/vortex/utils/ros/ros_conversions.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,6 +126,27 @@ inline vortex::utils::types::Pose ros_pose_to_pose(
return p;
}

/**
* @brief Converts a ROS geometry_msgs::msg::Pose to an internal Pose type.
* @param pose ROS pose message
* @return vortex::utils::types::PoseEuler Internal pose representation
*/
inline vortex::utils::types::PoseEuler ros_pose_to_pose_euler(
const geometry_msgs::msg::Pose& pose) {
vortex::utils::types::PoseEuler p;
p.x = pose.position.x;
p.y = pose.position.y;
p.z = pose.position.z;

Eigen::Quaterniond q(pose.orientation.w, pose.orientation.x,
pose.orientation.y, pose.orientation.z);
Eigen::Vector3d euler = vortex::utils::math::quat_to_euler(q);
p.roll = euler(0);
p.pitch = euler(1);
p.yaw = euler(2);
return p;
}

/**
* @brief Converts a ROS geometry_msgs::msg::Pose to an internal Pose vector
* type.
Expand Down Expand Up @@ -203,21 +224,20 @@ inline std::vector<vortex::utils::types::Pose> ros_to_pose_vec(
}

/**
* @brief Converts a ROS geometry_msgs::msg::TwistWithCovarianceStamped to an
* @brief Converts a ROS geometry_msgs::msg::Twist to an
* internal Twist type.
* @param twist_msg geometry_msgs::msg::TwistWithCovarianceStamped
* @param twist_msg geometry_msgs::msg::Twist
* @return vortex::utils::types::Twist Internal twist representation
*/
inline vortex::utils::types::Twist ros_twist_cov_msg_to_twist(
const geometry_msgs::msg::TwistWithCovarianceStamped& twist_msg) {
const auto& t = twist_msg.twist.twist;
inline vortex::utils::types::Twist ros_twist_to_twist(
const geometry_msgs::msg::Twist& twist_msg) {
return vortex::utils::types::Twist{
.u = t.linear.x,
.v = t.linear.y,
.w = t.linear.z,
.p = t.angular.x,
.q = t.angular.y,
.r = t.angular.z,
.u = twist_msg.linear.x,
.v = twist_msg.linear.y,
.w = twist_msg.linear.z,
.p = twist_msg.angular.x,
.q = twist_msg.angular.y,
.r = twist_msg.angular.z,
};
}

Expand Down
Loading