进一步信息
以下是我尝试读取机器人位姿的代码：
#include "robot_move_test/current_endeffector_pose_publisher.h"
CurrentEndeffectorPosePublisher::CurrentEndeffectorPosePublisher(const rclcpp::NodeOptions &options)
: Node("CurrentEndeffectorPosePublisher", options) {
RCLCPP_INFO(this->get_logger(), "CurrentEndeffectorPosePublisher node started.");
// Create a publisher for the end-effector pose
pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("endeffector_pose", 10);
// Create a timer to periodically publish the pose
timer_ = this->create_wall_timer(
std::chrono::milliseconds(1200), // Publish every 1.2 seconds
std::bind(&CurrentEndeffectorPosePublisher::publishPose, this));
}
void CurrentEndeffectorPosePublisher::initializeMoveGroup() {
RCLCPP_INFO(this->get_logger(), "Initializing MoveGroupInterface...");
try {
// Initialize MoveGroupInterface
move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(shared_from_this(), "panda_arm");
RCLCPP_INFO(this->get_logger(), "MoveGroupInterface initialized.");
} catch (const std::exception &e) {
RCLCPP_ERROR(this->get_logger(), "Exception during MoveGroupInterface initialization: %s", e.what());
} catch (...) {
RCLCPP_ERROR(this->get_logger(), "Unknown exception during MoveGroupInterface initialization.");
}
}
void CurrentEndeffectorPosePublisher::publishPose() {
if (!move_group_) {
RCLCPP_WARN(this->get_logger(), "MoveGroupInterface not initialized yet!");
return;
}
try {
auto current_pose = move_group_->getCurrentPose();
pose_publisher_->publish(current_pose);
RCLCPP_INFO(this->get_logger(), "Current Endeffector Pose: pos_x=%.3f, pos_y=%.3f, pos_z=%.3f, quat_x=%.3f, quat_y=%.3f, quat_z=%.3f, quat_w=%.3f",
current_pose.pose.position.x,
current_pose.pose.position.y,
current_pose.pose.position.z,
current_pose.pose.orientation.x,
current_pose.pose.orientation.y,
current_pose.pose.orientation.z,
current_pose.pose.orientation.w);
} catch (const std::exception &e) {
RCLCPP_ERROR(this->get_logger(), "Exception during publishPose: %s", e.what());
} catch (...) {
RCLCPP_ERROR(this->get_logger(), "Unknown exception during publishPose.");
}
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
try {
auto node = std::make_shared<CurrentEndeffectorPosePublisher>(rclcpp::NodeOptions());
node->initializeMoveGroup(); // Initialize MoveGroupInterface after the node is fully constructed
rclcpp::spin(node);
} catch (const std::exception &e) {
std::cerr << "Exception in main: " << e.what() << std::endl;
} catch (...) {
std::cerr << "Unknown exception in main." << std::endl;
}
rclcpp::shutdown();
return 0;
}
