みなさまこんにちわ。
GithubのほうにIssueを投げようかと思いましたが、どちらで上げればよいのか判断が付かなかった為こちらでご相談させて頂ければと思います。
背景
現在choreonoidをロボットの状態表示用ビューワとして利用するために、シミュレータ上のロボットの位置を任意の座標へ変更する処理をROS2とプラグインを用いて実装しております。
より詳細な方針としましては、ROS2でロボットの位置と姿勢情報（geometory_msgs::msg::PoseStamped）をサブスクライブし、setPositionにてシミュレータ上のロボットのルートリンクの位置を更新するというような単純なものです。
環境
・22.04.4 LTS (Jammy Jellyfish)
・ROS Noetic Ninjemys
・choreonoid v2.2.0
commit 0c234a35d0a58dfd2a7b773d6d3c18ea927ef76a
・choreonoid_ros commit 06787c67a6deeb107dca859843d2676572542951
発生事象について
setPosition()で位置をセットし、以下コードでロボットの位置の更新中に例外発生で落ちてしまうことが有ります（毎回ではない）。
bodyItem->notifyKinematicStateChange(true); // 位置の更新
その際にターミナル上には下記のようなメッセージが表示される場合もあれば、何も表示されない場合もあります。
double free or corruption (fasttop)
malloc(): unaligned tcache chunk detected
また、以下の場合もあります。
Could not parse stylesheet of object QDoubleSpinBox( 0xXXXXXXXXX)
malloc_consolidate(): unaligned fastbin chunk detected
つづいてスタックトレースの結果です。
choreonoidのプラグインもROS2もまだ触り始めで、どのように実装していけば良いのか正直よく分かってません。一先ずchoreonoid_rosのBodyROS2Itemを参考に実装しました。
また、当初SimpleControllerでの実装も考えましたが、将来的にプロパティアイテムでのパラメータの設定や障害物との接触判定などの実装を考えており、プラグインで実装する方針としております。
問題のコード
PoCコード
#include <cnoid/Plugin>
#include <cnoid/ItemManager>
#include <cnoid/Item>
#include <cnoid/RootItem>
#include <cnoid/BodyItem>
#include <cnoid/ItemList>
#include <cnoid/MessageView>
#include <cnoid/MessageOut>
#include <fmt/format.h>
// ROS2 library header
#include <rclcpp/rclcpp.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
using namespace std;
using namespace fmt;
using namespace cnoid;
class BodyPositionItem : public Item
{
// choreonoid
BodyItem* bodyItem;
Isometry3 position;
// ros
rclcpp::Node::SharedPtr rosNode;
std::unique_ptr<rclcpp::executors::SingleThreadedExecutor> executor;
std::thread executorThread;
rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_pose_;
public:
template<typename T>
void poseMsgToEigenImpl(const geometry_msgs::msg::Pose &m, T &e)
{
e = Eigen::Translation3d(m.position.x,
m.position.y,
m.position.z) *
Eigen::Quaterniond(m.orientation.w,
m.orientation.x,
m.orientation.y,
m.orientation.z);
}
BodyPositionItem()
{
bodyItem = nullptr;
position.setIdentity();
}
BodyPositionItem(const BodyPositionItem& org)
: Item(org)
{
bodyItem = nullptr;
position = org.position;
}
virtual Item* doDuplicate() const override
{
return new BodyPositionItem(*this);
}
virtual void onTreePathChanged() override
{
auto newBodyItem = findOwnerItem<BodyItem>();
if(newBodyItem && newBodyItem != bodyItem){
bodyItem = newBodyItem;
mvout()
<< format("BodyPositionItem \"{0}\" has been attached to {1}.",
name(), bodyItem->name())
<< endl;
// ROSノードの削除
if(executor)
{
executor->cancel();
if(executorThread.joinable())
{ // joinableでチェック
executorThread.join();
}
executor->remove_node(rosNode);
executor.reset();
}
// ROSノードの作成
if(rosNode==nullptr)
{ // リセット完了の場合
// ノード初期化
rosNode = std::make_shared<rclcpp::Node>("test_node");
executor = std::make_unique<rclcpp::executors::SingleThreadedExecutor>();
executor->add_node(rosNode);
executorThread = std::thread([this](){ executor->spin(); });
}
// サブスクライバ
sub_pose_ = rosNode->create_subscription<geometry_msgs::msg::PoseStamped>("robot_pose", rclcpp::QoS(1), std::bind(&BodyPositionItem::recvRobotPositionCb, this, std::placeholders::_1));
}
}
void recvRobotPositionCb(const geometry_msgs::msg::PoseStamped &msg)
{
RCLCPP_INFO(rosNode->get_logger(), "-------- UPDATE POSE CALLBACK START --------\n");
geometry_msgs::msg::TransformStamped transform;
RCLCPP_INFO(rosNode->get_logger(), "-------- dotransform --------\n");
geometry_msgs::msg::PoseStamped transformed_pose;
tf2::doTransform(msg, transformed_pose, transform);
// geometry_msgs::msg::Pose⇒Eigen::Isometry3d変換
Eigen::Isometry3d T = Eigen::Translation3d( transformed_pose.pose.position.x,
transformed_pose.pose.position.y,
transformed_pose.pose.position.z) *
Eigen::Quaterniond( transformed_pose.pose.orientation.w,
transformed_pose.pose.orientation.x,
transformed_pose.pose.orientation.y,
transformed_pose.pose.orientation.z);
// ルートリンクの更新
RCLCPP_INFO(rosNode->get_logger(), "-------- setPosition --------\n");
bodyItem->body()->rootLink()->setPosition(T); // 位置をセット
RCLCPP_INFO(rosNode->get_logger(), "-------- notifyKinematicStateChange --------\n");
bodyItem->notifyKinematicStateChange(true); // 位置の更新
RCLCPP_INFO(rosNode->get_logger(), "-------- UPDATE POSE CALLBACK END --------\n");
}
};
class DevGuidePlugin : public Plugin
{
public:
DevGuidePlugin()
: Plugin("DevGuide")
{
require("Body");
}
virtual bool initialize() override
{
itemManager()
.registerClass<BodyPositionItem>("BodyPositionItem")
.addCreationPanel<BodyPositionItem>();
return true;
}
};
CNOID_IMPLEMENT_PLUGIN_ENTRY(DevGuidePlugin)
何かお気づきの点や、アドバイス等頂けますと幸いです。