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

19
src/drawing_controller/drawing_controller/drawing_controller.py
View File

@@ -2,7 +2,7 @@
"""Sends motions individually to robot_controller""" """Sends motions individually to robot_controller"""
import rclpy import rclpy
from geometry_msgs.msg import Pose, PoseStamped from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion
from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.callback_groups import ReentrantCallbackGroup
from rclpy.node import Node from rclpy.node import Node
from rclpy.qos import QoSProfile from rclpy.qos import QoSProfile
@@ -103,26 +103,35 @@ class DrawingController(Node):
feedback = feedback_msg.feedback feedback = feedback_msg.feedback
self.get_logger().info('Received feedback: {0}'.format(feedback)) self.get_logger().info('Received feedback: {0}'.format(feedback))
def append_point(self, motion, point): def append_point(self, motion, point, height):
p = Pose() p = Pose()
#self.get_logger().info('Appending point:{} {}'.format(point[0], point[1]))
p.position.x = point[0] p.position.x = point[0]
p.position.y = point[1] p.position.y = point[1]
p.position.z = 0.1 p.position.z = height
q = quaternion_from_euler(0.0, math.pi, 0.0) q = quaternion_from_euler(0.0, math.pi, 0.0)
p.orientation = Quaternion()
p.orientation.x = q[0] p.orientation.x = q[0]
p.orientation.y = q[1] p.orientation.y = q[1]
p.orientation.z = q[2] p.orientation.z = q[2]
p.orientation.w = q[3] p.orientation.w = q[3]
ps = PoseStamped() ps = PoseStamped()
ps.pose = p
motion.path.append(ps) motion.path.append(ps)
def timer_callback(self): def timer_callback(self):
next_line = self.lines[self.i] next_line = self.lines[self.i]
motion = Motion() motion = Motion()
self.append_point(motion, self.map_point(float(next_line[0][0]),float(next_line[0][1]))) p1 = self.map_point(next_line[0][0],next_line[0][1])
self.append_point(motion, self.map_point(float(next_line[1][0]),float(next_line[1][1]))) p2 = self.map_point(next_line[1][0],next_line[1][1])
self.get_logger().info('Drawing line with p1:{} p2:{}'.format(p1,p2))
self.append_point(motion, p1, 0.5)
self.append_point(motion, p1, 0.1)
self.append_point(motion, p2, 0.1)
self.append_point(motion, p2, 0.5)
self.i = (self.i + 1) % len(self.lines) self.i = (self.i + 1) % len(self.lines)
self.get_logger().info('Executing motion:{}'.format(motion.path))
self.send_goal(motion) self.send_goal(motion)

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 feedback = std::make_shared<robot_interfaces::action::ExecuteMotion::Feedback>();
auto result = std::make_shared<robot_interfaces::action::ExecuteMotion::Result>(); 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) { for (int i = 1; (i <= 10) && rclcpp::ok(); ++i) {
// Check if there is a cancel request // Check if there is a cancel request