Gstreamer Aruco Detection - #45
Conversation
…/keyboard_auto
| RCLCPP_INFO(this->get_logger(), "Target pose: x=%.3f y=%.3f z=%.3f", | ||
| target_pose.pose.position.x, | ||
| target_pose.pose.position.y, | ||
| target_pose.pose.position.z); |
There was a problem hiding this comment.
Assuming this is a typo?
| rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr key_pose_sub_; | ||
| rclcpp::Subscription<std_msgs::msg::String>::SharedPtr string_sub_; | ||
| rclcpp::Publisher<std_msgs::msg::String>::SharedPtr target_key_pub_; | ||
| std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_; | ||
| std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> moveit_executor_; | ||
| std::thread moveit_spin_thread_; | ||
| std::thread typing_thread_; | ||
| geometry_msgs::msg::PoseStamped stored_pose_; | ||
| geometry_msgs::msg::PoseStamped home_pose_; | ||
| geometry_msgs::msg::PoseStamped latest_key_pose_; | ||
| std::atomic<bool> is_typing_{false}; | ||
| std::mutex pose_mutex_; | ||
| std::condition_variable pose_cv_; | ||
| bool pose_ready_{false}; |
There was a problem hiding this comment.
It would be better to move these declarations to a dedicated header file, just for cleanliness
There was a problem hiding this comment.
Does this serve a purpose anymore now that we have the moveit node?
| import threading | ||
| import time | ||
|
|
||
| gstreamer_pipeline = ('udpsrc port=40627 caps="application/x-rtp, media=(string)video, clock-rate=(int)90000, encoding-name=(string)H265" ! rtpjitterbuffer latency=50 ! rtph265depay ! h265parse ! queue max-size-buffers=20 max-size-time=0 max-size-bytes=0 leaky=downstream ! avdec_h265 ! videoconvert ! appsink') |
There was a problem hiding this comment.
Note, will need to test this eventually
| "moveit_testing", | ||
| rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)) | ||
| { | ||
| key_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>( |
There was a problem hiding this comment.
Does this need to be a PoseStamped? Can't it just be a Pose
| self.key_target_pub = self.create_publisher(PoseStamped, 'key_target_pose', 1) | ||
| self.target_key_sub = self.create_subscription( | ||
| String, | ||
| 'target_key', | ||
| self.target_key_callback, | ||
| 10 | ||
| ) |
There was a problem hiding this comment.
If we transition to using service calls, both of these can be replaced with a single service, although it may need to be a custom interface. In this case, the request is of type string or char and the response is a PoseStamped or Pose.
| // Publish key | ||
| auto key_msg = std_msgs::msg::String(); | ||
| key_msg.data = key; | ||
| target_key_pub_->publish(key_msg); | ||
|
|
||
| // Wait for ArUco to respond | ||
| std::unique_lock<std::mutex> lock(pose_mutex_); | ||
| pose_cv_.wait(lock, [this] { return pose_ready_; }); | ||
|
|
||
| plan_and_execute(latest_key_pose_); | ||
| go_home(); |
There was a problem hiding this comment.
Synchronizing the 2 nodes like this does work, but I think it's easier to use a service request here. So instead of this, you can replace it with something along the lines of
...
auto result_future = client->async_send_request(request);
if (rclcpp::spin_until_future_complete(this, result_future) ==
rclcpp::FutureReturnCode::SUCCESS)
{
auto result = result_future.get();
plan_and_execute(result->key_pose);
go_home();
}
...And change /target_key to be a service rather than a topic.
What do you think @WillScott9214 ?
| void go_home() | ||
| { | ||
| std::vector<double> home_joints = {0.0, 0.0, 0.0, 0.0}; | ||
| move_group_->setJointValueTarget(home_joints); | ||
| move_group_->setPlanningTime(10.0); | ||
|
|
||
| moveit::planning_interface::MoveGroupInterface::Plan my_plan; | ||
| bool success = (move_group_->plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS); | ||
| if (success) { | ||
| move_group_->execute(my_plan); | ||
| } | ||
| } |
There was a problem hiding this comment.
"Home" may need to be better defined, because we don't want the arm to go back to its zero position (it'll be too high up to see the keyboard)
2 participants
No description provided.