Skip to content

Commit 73a3747

Browse files
CurryMonstersCurryMonsters
committed
added degtorad converstion in waypoint-utils
1 parent 3fd73e2 commit 73a3747

2 files changed

Lines changed: 87 additions & 8 deletions

File tree

‎.vscode/settings.json‎

Lines changed: 73 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,73 @@
1+
{
2+
"ROS2.distro": "humble",
3+
"python.autoComplete.extraPaths": [
4+
"/home/hinthujan/ros2_ws/install/yasmin_editor/local/lib/python3.10/dist-packages",
5+
"/home/hinthujan/ros2_ws/install/yasmin_demos/local/lib/python3.10/dist-packages",
6+
"/home/hinthujan/ros2_ws/install/yasmin_factory/local/lib/python3.10/dist-packages",
7+
"/home/hinthujan/ros2_ws/install/yasmin_viewer/local/lib/python3.10/dist-packages",
8+
"/home/hinthujan/ros2_ws/install/yasmin_ros/local/lib/python3.10/dist-packages",
9+
"/home/hinthujan/ros2_ws/install/yasmin_msgs/local/lib/python3.10/dist-packages",
10+
"/home/hinthujan/ros2_ws/install/yasmin/lib/python3.10/site-packages",
11+
"/home/hinthujan/ros2_ws/install/velocity_controller_lqr/local/lib/python3.10/dist-packages",
12+
"/home/hinthujan/ros2_ws/install/vortex_utils_ros/local/lib/python3.10/dist-packages",
13+
"/home/hinthujan/ros2_ws/install/vortex_utils/local/lib/python3.10/dist-packages",
14+
"/home/hinthujan/ros2_ws/install/vortex_msgs/local/lib/python3.10/dist-packages",
15+
"/home/hinthujan/ros2_ws/install/stonefish_ros2/local/lib/python3.10/dist-packages",
16+
"/home/hinthujan/ros2_ws/install/my_robot_controller/lib/python3.10/site-packages",
17+
"/home/hinthujan/ros2_ws/build/keyboard_joy",
18+
"/home/hinthujan/ros2_ws/install/keyboard_joy/lib/python3.10/site-packages",
19+
"/home/hinthujan/ros2_ws/install/joystick_interface_auv/local/lib/python3.10/dist-packages",
20+
"/home/hinthujan/ros2_ws/install/btcpp_ros2_interfaces/local/lib/python3.10/dist-packages",
21+
"/home/hinthujan/ros2_ws/install/auv_setup/local/lib/python3.10/dist-packages",
22+
"/opt/ros/humble/lib/python3.10/site-packages",
23+
"/opt/ros/humble/local/lib/python3.10/dist-packages",
24+
"",
25+
"/opt/ros/humble/lib/yasmin_factory",
26+
"/opt/ros/humble/lib/yasmin_factory",
27+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
28+
"/opt/ros/humble/lib/yasmin_factory",
29+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
30+
"/opt/ros/humble/lib/yasmin_factory",
31+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
32+
"/opt/ros/humble/lib/yasmin_factory",
33+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
34+
"/opt/ros/humble/lib/yasmin_factory",
35+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
36+
"/opt/ros/humble/lib/yasmin_factory"
37+
],
38+
"python.analysis.extraPaths": [
39+
"/home/hinthujan/ros2_ws/install/yasmin_editor/local/lib/python3.10/dist-packages",
40+
"/home/hinthujan/ros2_ws/install/yasmin_demos/local/lib/python3.10/dist-packages",
41+
"/home/hinthujan/ros2_ws/install/yasmin_factory/local/lib/python3.10/dist-packages",
42+
"/home/hinthujan/ros2_ws/install/yasmin_viewer/local/lib/python3.10/dist-packages",
43+
"/home/hinthujan/ros2_ws/install/yasmin_ros/local/lib/python3.10/dist-packages",
44+
"/home/hinthujan/ros2_ws/install/yasmin_msgs/local/lib/python3.10/dist-packages",
45+
"/home/hinthujan/ros2_ws/install/yasmin/lib/python3.10/site-packages",
46+
"/home/hinthujan/ros2_ws/install/velocity_controller_lqr/local/lib/python3.10/dist-packages",
47+
"/home/hinthujan/ros2_ws/install/vortex_utils_ros/local/lib/python3.10/dist-packages",
48+
"/home/hinthujan/ros2_ws/install/vortex_utils/local/lib/python3.10/dist-packages",
49+
"/home/hinthujan/ros2_ws/install/vortex_msgs/local/lib/python3.10/dist-packages",
50+
"/home/hinthujan/ros2_ws/install/stonefish_ros2/local/lib/python3.10/dist-packages",
51+
"/home/hinthujan/ros2_ws/install/my_robot_controller/lib/python3.10/site-packages",
52+
"/home/hinthujan/ros2_ws/build/keyboard_joy",
53+
"/home/hinthujan/ros2_ws/install/keyboard_joy/lib/python3.10/site-packages",
54+
"/home/hinthujan/ros2_ws/install/joystick_interface_auv/local/lib/python3.10/dist-packages",
55+
"/home/hinthujan/ros2_ws/install/btcpp_ros2_interfaces/local/lib/python3.10/dist-packages",
56+
"/home/hinthujan/ros2_ws/install/auv_setup/local/lib/python3.10/dist-packages",
57+
"/opt/ros/humble/lib/python3.10/site-packages",
58+
"/opt/ros/humble/local/lib/python3.10/dist-packages",
59+
"",
60+
"/opt/ros/humble/lib/yasmin_factory",
61+
"/opt/ros/humble/lib/yasmin_factory",
62+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
63+
"/opt/ros/humble/lib/yasmin_factory",
64+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
65+
"/opt/ros/humble/lib/yasmin_factory",
66+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
67+
"/opt/ros/humble/lib/yasmin_factory",
68+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
69+
"/opt/ros/humble/lib/yasmin_factory",
70+
"/home/hinthujan/ros2_ws/install/yasmin_factory/lib/yasmin_factory",
71+
"/opt/ros/humble/lib/yasmin_factory"
72+
]
73+
}

‎vortex_utils/src/waypoint_utils.cpp‎

Lines changed: 14 additions & 8 deletions
Original file line numberDiff line numberDiff line change
@@ -32,7 +32,7 @@ WaypointMode load_mode(const YAML::Node& node) {
3232
}
3333
}
3434

35-
Pose load_pose_for_mode(const YAML::Node& node, WaypointMode mode) {
35+
Pose load_pose_for_mode(const YAML::Node& node, WaypointMode mode) { //
3636
Pose pose; // defaults: x=y=z=0, qw=1, qx=qy=qz=0
3737

3838
const bool needs_position = (mode == WaypointMode::FULL_POSE ||
@@ -48,9 +48,12 @@ Pose load_pose_for_mode(const YAML::Node& node, WaypointMode mode) {
4848
}
4949

5050
if (needs_orientation) {
51-
const double roll = node["orientation"]["roll"].as<double>();
52-
const double pitch = node["orientation"]["pitch"].as<double>();
53-
const double yaw = node["orientation"]["yaw"].as<double>();
51+
const double roll =
52+
node["orientation"]["roll"].as<double>() * (M_PI / 180.0);
53+
const double pitch =
54+
node["orientation"]["pitch"].as<double>() * (M_PI / 180.0);
55+
const double yaw =
56+
node["orientation"]["yaw"].as<double>() * (M_PI / 180.0);
5457
const Eigen::Quaterniond q =
5558
vortex::utils::math::euler_to_quat(roll, pitch, yaw);
5659
pose.qw = q.w();
@@ -135,7 +138,7 @@ Pose apply_pose_offset(const Pose& base, const Pose& offset) {
135138
}
136139

137140
Pose load_pose_from_yaml(const std::string& file_path,
138-
const std::string& identifier) {
141+
const std::string& identifier) { //
139142
YAML::Node root = YAML::LoadFile(file_path);
140143

141144
if (!root[identifier]) {
@@ -149,9 +152,12 @@ Pose load_pose_from_yaml(const std::string& file_path,
149152
const double y = pose["position"]["y"].as<double>();
150153
const double z = pose["position"]["z"].as<double>();
151154

152-
const double roll = pose["orientation"]["roll"].as<double>();
153-
const double pitch = pose["orientation"]["pitch"].as<double>();
154-
const double yaw = pose["orientation"]["yaw"].as<double>();
155+
const double roll = pose["orientation"]["roll"].as<double>() *
156+
(M_PI / 180.0);
157+
const double pitch = pose["orientation"]["pitch"].as<double>() *
158+
(M_PI / 180.0);
159+
const double yaw = pose["orientation"]["yaw"].as<double>() *
160+
(M_PI / 180.0);
155161

156162
const Eigen::Quaterniond q =
157163
vortex::utils::math::euler_to_quat(roll, pitch, yaw);

0 commit comments

Comments
 (0)