nic/drawing-robot-ros2
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fix 0 pose bug

Browse Source
This commit is contained in:
Nicolas Hiillos
2022-12-21 11:59:47 +02:00
parent 7aa62072fe
commit e980579365
2 changed files with 21 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

7
src/robot_controller/src/cpp/dummy_controller.cpp
View File

@@ -24,6 +24,13 @@ class DummyController : public RobotController
auto feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>();
for (auto p : goal->motion.path)
{
auto pp = p.pose;
RCLCPP_INFO(this->get_logger(), "Position x:%f y:%f z:%f",pp.position.x,pp.position.y,pp.position.z);
RCLCPP_INFO(this->get_logger(), "Orientation w:%f x:%f y:%f z:%f", pp.orientation.w, pp.orientation.x,pp.orientation.y,pp.orientation.z);
// W:%f X:%f Y:%f Z:%f
}
for (int i = 1; (i <= 10) && rclcpp::ok(); ++i) {
// Check if there is a cancel request