为什么选择CoppeliaSim，主流机器人仿真软件个人使用经验已经有20年左右。从远古到如今。
当然也有一些参考文献指出的：
使用大模型撰写移动机器人模拟器（CoppeliaSim、Gazebo、MORSE和Webots）的定量比较研究报告
淘汰或者落后的技术内容没有学习的必要。
但是，提高学习和研究的效率这种方法永远是有收益的。
基于CoppeliaSim_Edu_V4_7_0_rev4_Ubuntu24_04和ROS2 Jazzy已经完全开发测试结束。后续会逐步更新。
问题描述：
版本：
CoppeliaSim 4.7.0
ROS Jazzy版本：
ros-jazzy-desktop-full is already the newest version (0.11.0-1noble.20240731.183615).
出错信息：
The Python interpreter could not handle the wrapper script (or communication between the launched subprocess and CoppeliaSim could not be established via sockets). Make sure that the Python modules 'cbor2' and 'zmq' are properly installed, e.g. via: $ python3 -m pip install pyzmq cbor2. Additionally, you can try adjusting the value of startTimeout in lua/pythonWrapperV2.lua, at the top of the file
解决：
error: externally-managed-environment
× This environment is externally managed
╰─> To install Python packages system-wide, try apt install
python3-xyz, where xyz is the package you are trying to
install.
If you wish to install a non-Debian-packaged Python package,
create a virtual environment using python3 -m venv path/to/venv.
Then use path/to/venv/bin/python and path/to/venv/bin/pip. Make
sure you have python3-full installed.
If you wish to install a non-Debian packaged Python application,
it may be easiest to use pipx install xyz, which will manage a
virtual environment for you. Make sure you have pipx installed.
See /usr/share/doc/python3.12/README.venv for more information.
note: If you believe this is a mistake, please contact your Python installation or OS distribution provider. You can override this, at the risk of breaking your Python installation or OS, by passing --break-system-packages.
hint: See PEP 668 for the detailed specification.
命令替换：
148 sudo apt install python-is-python3
149 python3 -m pip install pyzmq cbor2
150 sudo apt install python3-pip
151 python3 -m pip install pyzmq cbor2
152 sudo apt install python3-zmq
153 python3 -m pip install pyzmq cbor2
154 sudo apt install python3-cbor2
注意：加租部分。
报错消失：
做下视频框有显示了。
ROS2 rqt
ok。
第二个例子
总结
在您遇到的问题中，您已经成功地解决了在Ubuntu系统下使用CoppeliaSim（现称为CoppeliaRobotics）与ROS 2（特别是ROS 2 Jazzy）集成时遇到的Python环境和依赖问题。这里，我将概述一下您已经采取的步骤，并提供一些额外的建议和优化方法，以帮助您更好地整合和使用这两个强大的工具。
已采取的步骤
- 确认Python依赖：
- 您通过
python3 -m pip install pyzmq cbor2安装了必要的Python库（pyzmq和cbor2），这些库对于CoppeliaSim与Python的通信至关重要。
- 解决系统Python包管理问题：
- 您注意到系统Python环境是外部管理的，因此建议使用系统包管理器（如
apt）或创建虚拟环境来管理非Debian包的Python包。您最终通过apt安装了python3-zmq（尽管这可能不是必须的，因为pip已经安装了pyzmq），并且确认pyzmq和cbor2通过pip安装成功。
- 调整并测试：
- 修改了
lua/pythonWrapperV2.lua中的startTimeout值（如果需要），以确保有足够的时间来启动Python脚本和建立通信。 - 重启CoppeliaSim和ROS 2环境，并确认错误已解决，视频框可以正确显示。
进一步的建议
- 使用虚拟环境：
- 为了避免系统级Python包的冲突和权限问题，建议使用Python虚拟环境。您可以使用
venv或conda来创建一个隔离的Python环境，并在其中安装所有必要的依赖项。
bash复制代码
| |
| |
|
- 更新和兼容性检查：
- 确保您的CoppeliaRobotics和ROS 2 Jazzy版本相互兼容。检查CoppeliaRobotics的文档和社区论坛，以获取关于与ROS 2集成的最新信息和建议。
- 查看ROS 2和CoppeliaSim的集成教程：
- 官方教程和社区提供的示例代码是学习如何正确集成这两个工具的宝贵资源。这些教程通常涵盖从设置环境到运行示例仿真的所有步骤。
- 调试和日志记录：
- 在集成过程中，如果遇到问题，增加日志记录并仔细查看错误消息和堆栈跟踪可以帮助快速定位问题。
- 考虑使用ROS 2的CoppeliaSim桥接包：
- 查找是否有现成的ROS 2包可以简化CoppeliaSim与ROS 2的集成。这些包可能提供了高级接口，用于控制仿真环境中的机器人和传感器。
- 社区支持：
- 利用CoppeliaRobotics和ROS 2的社区论坛和问答网站（如Stack Overflow）来获取帮助和支持。
通过这些步骤和建议，您应该能够更有效地将CoppeliaSim与ROS 2集成，并利用这两个工具的强大功能进行机器人仿真和研究。
代码：
#include <sim_ros2_interface.h>
#include <simPlusPlus/Plugin.h>
#include <simPlusPlus/Handles.h>
#include <cstdlib>
#include <functional>
using namespace std::placeholders;
#include <boost/type_erasure/any_cast.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <sensor_msgs/image_encodings.hpp>
#if image_transport_FOUND
#include <image_transport/image_transport.hpp>
#endif
//#include <cv_bridge/cv_bridge.h>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
using namespace std::chrono_literals;
struct unsupported_type : public std::exception
{
std::string message;
unsupported_type(const std::string &w, const std::string &t)
{
std::stringstream ss;
ss << "Unsupported " << w << " type '" << t << "'. You may want to add it to meta/interfaces.txt and recompile the ROS2Interface plugin.";
message = ss.str();
}
~unsupported_type() throw() {}
const char * what() const throw() {return message.c_str();}
};
class Plugin : public sim::Plugin
{
public:
void onInit()
{
if(!initialize())
throw std::runtime_error("failed to initialize ROS2 node");
if(!registerScriptStuff())
throw std::runtime_error("failed to register script stuff");
setExtVersion("ROS2 Interface Plugin");
setBuildDate(BUILD_DATE);
}
void onCleanup()
{
shutdown();
}
void onInstancePass(const sim::InstancePassFlags &flags)
{
rclcpp::spin_some(node);
}
void onMainScriptAboutToBeCalled(int &out)
{
int stopSimulationRequestCounter = sim::getInt32Param(sim_intparam_stop_request_counter);
bool doNotRun = sim::getBoolParam(sim_boolparam_rosinterface_donotrunmainscript);
if(doNotRun > 0)
{
if(previousStopSimulationRequestCounter == -1)
previousStopSimulationRequestCounter = stopSimulationRequestCounter;
if(previousStopSimulationRequestCounter == stopSimulationRequestCounter)
out = 0; // this tells CoppeliaSim that we don't wanna execute the main script
}
else
previousStopSimulationRequestCounter = -1;
}
void onSimulationAboutToStart()
{
previousStopSimulationRequestCounter = -1;
}
void onScriptStateAboutToBeDestroyed(int scriptHandle, int scriptUid)
{
for(auto subscriptionProxy : subscriptionHandles.find(scriptHandle))
{
if(!subscriptionProxy->subscription.empty())
{
shutdownSubscription_in in1;
in1.subscriptionHandle = subscriptionProxy->handle;
shutdownSubscription_out out1;
shutdownSubscription(&in1, &out1);
}
#if image_transport_FOUND
if(subscriptionProxy->imageTransportSubscription)
{
imageTransportShutdownSubscription_in in1;
in1.subscriptionHandle = subscriptionProxy->handle;
imageTransportShutdownSubscription_out out1;
imageTransportShutdownSubscription(&in1, &out1);
}
#endif
}
for(auto publisherProxy : publisherHandles.find(scriptHandle))
{
if(!publisherProxy->publisher.empty())
{
shutdownPublisher_in in1;
in1.publisherHandle = publisherProxy->handle;
shutdownPublisher_out out1;
shutdownPublisher(&in1, &out1);
}
#if image_transport_FOUND
if(publisherProxy->imageTransportPublisher)
{
imageTransportShutdownPublisher_in in1;
in1.publisherHandle = publisherProxy->handle;
imageTransportShutdownPublisher_out out1;
imageTransportShutdownPublisher(&in1, &out1);
}
#endif
}
for(auto clientProxy : clientHandles.find(scriptHandle))
{
shutdownClient_in in1;
in1.clientHandle = clientProxy->handle;
shutdownClient_out out1;
shutdownClient(&in1, &out1);
}
for(auto serviceProxy : serviceHandles.find(scriptHandle))
{
shutdownService_in in1;
in1.serviceHandle = serviceProxy->handle;
shutdownService_out out1;
shutdownService(&in1, &out1);
}
for(auto actionClientProxy : actionClientHandles.find(scriptHandle))
{
shutdownActionClient_in in1;
in1.actionClientHandle = actionClientProxy->handle;
shutdownActionClient_out out1;
shutdownActionClient(&in1, &out1);
}
for(auto actionServerProxy : actionServerHandles.find(scriptHandle))
{
shutdownActionServer_in in1;
in1.actionServerHandle = actionServerProxy->handle;
shutdownActionServer_out out1;
shutdownActionServer(&in1, &out1);
}
}
bool shouldProxyBeDestroyedAfterSimulationStop(int scriptHandle)
{
if(sim::getSimulationState() == sim_simulation_stopped)
return false;
int property = sim::getScriptInt32Param(scriptHandle, sim_scriptintparam_type);
#if SIM_PROGRAM_FULL_VERSION_NB <= 4010003
if(property & sim_scripttype_threaded)
property -= sim_scripttype_threaded;
#else
if(property & sim_scripttype_threaded_old)
property -= sim_scripttype_threaded_old;
#endif
if(property == sim_scripttype_addon || property == sim_scripttype_addonfunction || property == sim_scripttype_customization)
return false;
return true;
}
static void ros_imtr_callback(const sensor_msgs::msg::Image::ConstSharedPtr &msg, SubscriptionProxy *subscriptionProxy, Plugin *plugin)
{
if(msg->is_bigendian)
{
std::cerr << "ros_imtr_callback: error: big endian image not supported" << std::endl;
return;
}
int data_len = msg->step * msg->height;
imageTransportCallback_in in_args;
imageTransportCallback_out out_args;
in_args.width = msg->width;
in_args.height = msg->height;
in_args.data.resize(data_len);
for(unsigned int i = 0; i < msg->height; i++)
{
int msg_idx = (msg->height - i - 1) * msg->step;
int buf_idx = i * msg->step;
for(unsigned int j = 0; j < msg->step; j++)
{
in_args.data[buf_idx + j] = msg->data[msg_idx + j];
}
}
if(!imageTransportCallback(subscriptionProxy->topicCallback.scriptId, subscriptionProxy->topicCallback.name.c_str(), &in_args, &out_args))
{
std::cerr << "ros_imtr_callback: error: failed to call callback" << std::endl;
return;
}
}
rclcpp::QoS get_qos(const std::optional<simros2_qos> &opt_qos)
{
if(!opt_qos.has_value())
return rclcpp::QoS {10};
const simros2_qos &qos = opt_qos.value();
rmw_qos_profile_t profile;
switch(qos.history)
{
case simros2_qos_history_policy_system_default:
profile.history = RMW_QOS_POLICY_HISTORY_SYSTEM_DEFAULT;
break;
case simros2_qos_history_policy_keep_last:
profile.history = RMW_QOS_POLICY_HISTORY_KEEP_LAST;
break;
case simros2_qos_history_policy_keep_all:
profile.history = RMW_QOS_POLICY_HISTORY_KEEP_ALL;
break;
}
profile.depth = qos.depth;
switch(qos.reliability)
{
case simros2_qos_reliability_policy_system_default:
profile.reliability = RMW_QOS_POLICY_RELIABILITY_SYSTEM_DEFAULT;
break;
case simros2_qos_reliability_policy_reliable:
profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
break;
case simros2_qos_reliability_policy_best_effort:
profile.reliability = RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT;
break;
}
switch(qos.durability)
{
case simros2_qos_durability_policy_system_default:
profile.durability = RMW_QOS_POLICY_DURABILITY_SYSTEM_DEFAULT;
break;
case simros2_qos_durability_policy_transient_local:
profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
break;
case simros2_qos_durability_policy_volatile:
profile.durability = RMW_QOS_POLICY_DURABILITY_VOLATILE;
break;
}
profile.deadline.sec = qos.deadline.sec;
profile.deadline.nsec = qos.deadline.nanosec;
profile.lifespan.sec = qos.lifespan.sec;
profile.lifespan.nsec = qos.lifespan.nanosec;
switch(qos.liveliness)
{
case simros2_qos_liveliness_policy_system_default:
profile.liveliness = RMW_QOS_POLICY_LIVELINESS_SYSTEM_DEFAULT;
break;
case simros2_qos_liveliness_policy_automatic:
profile.liveliness = RMW_QOS_POLICY_LIVELINESS_AUTOMATIC;
break;
case simros2_qos_liveliness_policy_manual_by_topic:
profile.liveliness = RMW_QOS_POLICY_LIVELINESS_MANUAL_BY_TOPIC;
break;
}
profile.liveliness_lease_duration.sec = qos.liveliness_lease_duration.sec;
profile.liveliness_lease_duration.nsec = qos.liveliness_lease_duration.nanosec;
profile.avoid_ros_namespace_conventions = qos.avoid_ros_namespace_conventions;
return rclcpp::QoS(rclcpp::QoSInitialization::from_rmw(profile), profile);
}
void createSubscription(createSubscription_in *in, createSubscription_out *out)
{
SubscriptionProxy *subscriptionProxy = new SubscriptionProxy();
subscriptionProxy->topicName = in->topicName;
subscriptionProxy->topicType = in->topicType;
subscriptionProxy->topicCallback.scriptId = in->_.scriptID;
subscriptionProxy->topicCallback.name = in->topicCallback;
if(0) {}
#include <sub_new.cpp>
else
{
throw unsupported_type("message", subscriptionProxy->topicType);
}
out->subscriptionHandle = subscriptionProxy->handle = subscriptionHandles.add(subscriptionProxy, in->_.scriptID);
}
void shutdownSubscription(shutdownSubscription_in *in, shutdownSubscription_out *out)
{
SubscriptionProxy *subscriptionProxy = subscriptionHandles.get(in->subscriptionHandle);
if(0) {}
#include <sub_del.cpp>
else
{
throw unsupported_type("message", subscriptionProxy->topicType);
}
delete subscriptionHandles.remove(subscriptionProxy);
}
void subscriptionTreatUInt8ArrayAsString(subscriptionTreatUInt8ArrayAsString_in *in, subscriptionTreatUInt8ArrayAsString_out *out)
{
SubscriptionProxy *subscriptionProxy = subscriptionHandles.get(in->subscriptionHandle);
subscriptionProxy->wr_opt.uint8array_as_string = true;
}
void createPublisher(createPublisher_in *in, createPublisher_out *out)
{
PublisherProxy *publisherProxy = new PublisherProxy();
publisherProxy->topicName = in->topicName;
publisherProxy->topicType = in->topicType;
if(0) {}
#include <pub_new.cpp>
else
{
throw unsupported_type("message", publisherProxy->topicType);
}
out->publisherHandle = publisherProxy->handle = publisherHandles.add(publisherProxy, in->_.scriptID);
}
void shutdownPublisher(shutdownPublisher_in *in, shutdownPublisher_out *out)
{
PublisherProxy *publisherProxy = publisherHandles.get(in->publisherHandle);
if(0) {}
#include <pub_del.cpp>
else
{
throw unsupported_type("message", publisherProxy->topicType);
}
delete publisherHandles.remove(publisherProxy);
}
void publisherTreatUInt8ArrayAsString(publisherTreatUInt8ArrayAsString_in *in, publisherTreatUInt8ArrayAsString_out *out)
{
PublisherProxy *publisherProxy = publisherHandles.get(in->publisherHandle);
publisherProxy->rd_opt.uint8array_as_string = true;
}
void publish(publish_in *in, publish_out *out)
{
PublisherProxy *publisherProxy = publisherHandles.get(in->publisherHandle);
sim::moveStackItemToTop(in->_.stackID, 0);
if(0) {}
#include <pub_publish.cpp>
else
{
throw unsupported_type("message", publisherProxy->topicType);
}
}
void createClient(createClient_in *in, createClient_out *out)
{
ClientProxy *clientProxy = new ClientProxy();
clientProxy->serviceName = in->serviceName;
clientProxy->serviceType = in->serviceType;
if(0) {}
#include <cli_new.cpp>
else
{
throw unsupported_type("service", clientProxy->serviceType);
}
out->clientHandle = clientProxy->handle = clientHandles.add(clientProxy, in->_.scriptID);
}
void shutdownClient(shutdownClient_in *in, shutdownClient_out *out)
{
ClientProxy *clientProxy = clientHandles.get(in->clientHandle);
if(0) {}
#include <cli_del.cpp>
else
{
throw unsupported_type("service", clientProxy->serviceType);
}
delete clientHandles.remove(clientProxy);
}
void clientTreatUInt8ArrayAsString(clientTreatUInt8ArrayAsString_in *in, clientTreatUInt8ArrayAsString_out *out)
{
ClientProxy *clientProxy = clientHandles.get(in->clientHandle);
clientProxy->rd_opt.uint8array_as_string = true;
clientProxy->wr_opt.uint8array_as_string = true;
}
void waitForService(waitForService_in *in, waitForService_out *out)
{
ClientProxy *clientProxy = clientHandles.get(in->clientHandle);
if(0) {}
#include <cli_wait.cpp>
else
{
throw unsupported_type("service", clientProxy->serviceType);
}
}
void call(call_in *in, call_out *out)
{
ClientProxy *clientProxy = clientHandles.get(in->clientHandle);
sim::moveStackItemToTop(in->_.stackID, 0);
if(0) {}
#include <cli_call.cpp>
else
{
throw unsupported_type("service", clientProxy->serviceType);
}
}
void createService(createService_in *in, createService_out *out)
{
ServiceProxy *serviceProxy = new ServiceProxy();
serviceProxy->serviceName = in->serviceName;
serviceProxy->serviceType = in->serviceType;
serviceProxy->serviceCallback.scriptId = in->_.scriptID;
serviceProxy->serviceCallback.name = in->serviceCallback;
if(0) {}
#include <srv_new.cpp>
else
{
throw unsupported_type("service", serviceProxy->serviceType);
}
out->serviceHandle = serviceProxy->handle = serviceHandles.add(serviceProxy, in->_.scriptID);
}
void shutdownService(shutdownService_in *in, shutdownService_out *out)
{
ServiceProxy *serviceProxy = serviceHandles.get(in->serviceHandle);
if(0) {}
#include <srv_del.cpp>
else
{
throw unsupported_type("service", serviceProxy->serviceType);
}
delete serviceHandles.remove(serviceProxy);
}
void serviceTreatUInt8ArrayAsString(serviceTreatUInt8ArrayAsString_in *in, serviceTreatUInt8ArrayAsString_out *out)
{
ServiceProxy *serviceProxy = serviceHandles.get(in->serviceHandle);
serviceProxy->rd_opt.uint8array_as_string = true;
serviceProxy->wr_opt.uint8array_as_string = true;
}
void createActionClient(createActionClient_in *in, createActionClient_out *out)
{
ActionClientProxy *actionClientProxy = new ActionClientProxy();
actionClientProxy->actionName = in->actionName;
actionClientProxy->actionType = in->actionType;
actionClientProxy->goalResponseCallback.scriptId = in->_.scriptID;
actionClientProxy->goalResponseCallback.name = in->goalResponseCallback;
actionClientProxy->feedbackCallback.scriptId = in->_.scriptID;
actionClientProxy->feedbackCallback.name = in->feedbackCallback;
actionClientProxy->resultCallback.scriptId = in->_.scriptID;
actionClientProxy->resultCallback.name = in->resultCallback;
if(0) {}
#include <actcli_new.cpp>
else
{
throw unsupported_type("action", actionClientProxy->actionType);
}
out->actionClientHandle = actionClientProxy->handle = actionClientHandles.add(actionClientProxy, in->_.scriptID);
}
void shutdownActionClient(shutdownActionClient_in *in, shutdownActionClient_out *out)
{
ActionClientProxy *actionClientProxy = actionClientHandles.get(in->actionClientHandle);
if(0) {}
#include <actcli_del.cpp>
else
{
throw unsupported_type("action", actionClientProxy->actionType);
}
delete actionClientHandles.remove(actionClientProxy);
}
void actionClientTreatUInt8ArrayAsString(actionClientTreatUInt8ArrayAsString_in *in, actionClientTreatUInt8ArrayAsString_out *out)
{
ActionClientProxy *actionClientProxy = actionClientHandles.get(in->actionClientHandle);
actionClientProxy->rd_opt.uint8array_as_string = true;
actionClientProxy->wr_opt.uint8array_as_string = true;
}
void sendGoal(sendGoal_in *in, sendGoal_out *out)
{
ActionClientProxy *actionClientProxy = actionClientHandles.get(in->actionClientHandle);
sim::moveStackItemToTop(in->_.stackID, 0);
if(0) {}
#include <actcli_sendGoal.cpp>
else
{
throw unsupported_type("action", actionClientProxy->actionType);
}
}
void cancelLastGoal(cancelLastGoal_in *in, cancelLastGoal_out *out)
{
ActionClientProxy *actionClientProxy = actionClientHandles.get(in->actionClientHandle);
if(0) {}
#include <actcli_cancelLastGoal.cpp>
else
{
throw unsupported_type("action", actionClientProxy->actionType);
}
}
void createActionServer(createActionServer_in *in, createActionServer_out *out)
{
ActionServerProxy *actionServerProxy = new ActionServerProxy();
actionServerProxy->actionName = in->actionName;
actionServerProxy->actionType = in->actionType;
actionServerProxy->handleGoalCallback.scriptId = in->_.scriptID;
actionServerProxy->handleGoalCallback.name = in->handleGoalCallback;
actionServerProxy->handleCancelCallback.scriptId = in->_.scriptID;
actionServerProxy->handleCancelCallback.name = in->handleCancelCallback;
actionServerProxy->handleAcceptedCallback.scriptId = in->_.scriptID;
actionServerProxy->handleAcceptedCallback.name = in->handleAcceptedCallback;
if(0) {}
#include <actsrv_new.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
out->actionServerHandle = actionServerProxy->handle = actionServerHandles.add(actionServerProxy, in->_.scriptID);
}
void shutdownActionServer(shutdownActionServer_in *in, shutdownActionServer_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_del.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
delete actionServerHandles.remove(actionServerProxy);
}
void actionServerTreatUInt8ArrayAsString(actionServerTreatUInt8ArrayAsString_in *in, actionServerTreatUInt8ArrayAsString_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
actionServerProxy->rd_opt.uint8array_as_string = true;
actionServerProxy->wr_opt.uint8array_as_string = true;
}
template<typename Action>
rclcpp_action::ServerGoalHandle<Action> * getGoalHandle(ActionServerProxy *actionServerProxy, const std::string &goalUUIDstr)
{
return getGoalHandle<Action>(actionServerProxy, goalUUIDfromString(goalUUIDstr));
}
template<typename Action>
rclcpp_action::ServerGoalHandle<Action> * getGoalHandle(ActionServerProxy *actionServerProxy, const rclcpp_action::GoalUUID &goalUUID)
{
auto actsrv = boost::any_cast< std::shared_ptr< rclcpp_action::Server< Action > > >(actionServerProxy->action_server);
auto it = actionServerProxy->goalHandles.find(goalUUID);
if(it == actionServerProxy->goalHandles.end())
throw sim::exception("goal handle '%s' does not exist", goalUUIDtoString(goalUUID));
auto goalHandleBase = it->second.get();
return dynamic_cast< rclcpp_action::ServerGoalHandle<Action>* >(goalHandleBase);
}
void cleanupTerminalGoalHandles(ActionServerProxy *actionServerProxy)
{
/* FIXME: crash when actionServerActionSucceed is called */ return;
for(auto it = actionServerProxy->goalHandles.begin(); it != actionServerProxy->goalHandles.end(); )
{
if(it->second->is_active())
++it;
else
actionServerProxy->goalHandles.erase(it);
}
}
void actionServerPublishFeedback(actionServerPublishFeedback_in *in, actionServerPublishFeedback_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_action_publish_feedback.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
}
void actionServerActionAbort(actionServerActionAbort_in *in, actionServerActionAbort_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_action_abort.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
cleanupTerminalGoalHandles(actionServerProxy);
}
void actionServerActionSucceed(actionServerActionSucceed_in *in, actionServerActionSucceed_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_action_succeed.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
cleanupTerminalGoalHandles(actionServerProxy);
}
void actionServerActionCanceled(actionServerActionCanceled_in *in, actionServerActionCanceled_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_action_canceled.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
cleanupTerminalGoalHandles(actionServerProxy);
}
void actionServerActionExecute(actionServerActionExecute_in *in, actionServerActionExecute_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_action_execute.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
cleanupTerminalGoalHandles(actionServerProxy);
}
void actionServerActionIsCanceling(actionServerActionIsCanceling_in *in, actionServerActionIsCanceling_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_action_is_canceling.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
}
void actionServerActionIsActive(actionServerActionIsActive_in *in, actionServerActionIsActive_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_action_is_active.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
}
void actionServerActionIsExecuting(actionServerActionIsExecuting_in *in, actionServerActionIsExecuting_out *out)
{
ActionServerProxy *actionServerProxy = actionServerHandles.get(in->actionServerHandle);
if(0) {}
#include <actsrv_action_is_executing.cpp>
else
{
throw unsupported_type("action", actionServerProxy->actionType);
}
}
void sendTransform(sendTransform_in *in, sendTransform_out *out)
{
geometry_msgs::msg::TransformStamped t;
read__geometry_msgs__msg__TransformStamped(in->_.stackID, &t);
tfbr->sendTransform(t);
}
void sendTransforms(sendTransforms_in *in, sendTransforms_out *out)
{
std::vector<geometry_msgs::msg::TransformStamped> v;
sim::moveStackItemToTop(in->_.stackID, 0);
int i = sim::getStackTableInfo(in->_.stackID, 0);
if(i < 0)
throw sim::exception("error reading input argument 1 (origin): expected array");
int oldsz = sim::getStackSize(in->_.stackID);
sim::unfoldStackTable(in->_.stackID);
int sz = (sim::getStackSize(in->_.stackID) - oldsz + 1) / 2;
for(int i = 0; i < sz; i++)
{
sim::moveStackItemToTop(in->_.stackID, oldsz - 1);
int j;
read__int32(in->_.stackID, &j);
sim::moveStackItemToTop(in->_.stackID, oldsz - 1);
geometry_msgs::msg::TransformStamped t;
read__geometry_msgs__msg__TransformStamped(in->_.stackID, &t);
v.push_back(t);
}
tfbr->sendTransform(v);
}
void imageTransportCreateSubscription(imageTransportCreateSubscription_in *in, imageTransportCreateSubscription_out *out)
{
#if image_transport_FOUND
SubscriptionProxy *subscriptionProxy = new SubscriptionProxy();
subscriptionProxy->topicName = in->topicName;
subscriptionProxy->topicType = "@image_transport";
subscriptionProxy->topicCallback.scriptId = in->_.scriptID;
subscriptionProxy->topicCallback.name = in->topicCallback;
subscriptionProxy->imageTransportSubscription = imtr->subscribe(in->topicName, in->queueSize, std::bind(ros_imtr_callback, _1, subscriptionProxy, this));
if(!subscriptionProxy->imageTransportSubscription)
{
throw sim::exception("failed creation of ROS ImageTransport subscription");
}
out->subscriptionHandle = subscriptionProxy->handle = subscriptionHandles.add(subscriptionProxy, in->_.scriptID);
#else
throw sim::exception("image_transport not available. please rebuild this plugin.");
#endif
}
void imageTransportShutdownSubscription(imageTransportShutdownSubscription_in *in, imageTransportShutdownSubscription_out *out)
{
SubscriptionProxy *subscriptionProxy = subscriptionHandles.get(in->subscriptionHandle);
//subscriptionProxy->imageTransportSubscription.shutdown();
delete subscriptionHandles.remove(subscriptionProxy);
}
void imageTransportCreatePublisher(imageTransportCreatePublisher_in *in, imageTransportCreatePublisher_out *out)
{
#if image_transport_FOUND
PublisherProxy *publisherProxy = new PublisherProxy();
publisherProxy->topicName = in->topicName;
publisherProxy->topicType = "@image_transport";
publisherProxy->imageTransportPublisher = imtr->advertise(in->topicName, in->queueSize);
if(!publisherProxy->imageTransportPublisher)
{
throw sim::exception("failed creation of ROS ImageTransport publisher");
}
out->publisherHandle = publisherProxy->handle = publisherHandles.add(publisherProxy, in->_.scriptID);
#else
throw sim::exception("image_transport not available. please rebuild this plugin.");
#endif
}
void imageTransportShutdownPublisher(imageTransportShutdownPublisher_in *in, imageTransportShutdownPublisher_out *out)
{
#if image_transport_FOUND
PublisherProxy *publisherProxy = publisherHandles.get(in->publisherHandle);
//publisherProxy->imageTransportPublisher.shutdown();
delete publisherHandles.remove(publisherProxy);
#else
throw sim::exception("image_transport not available. please rebuild this plugin.");
#endif
}
void imageTransportPublish(imageTransportPublish_in *in, imageTransportPublish_out *out)
{
#if image_transport_FOUND
PublisherProxy *publisherProxy = publisherHandles.get(in->publisherHandle);
sensor_msgs::msg::Image image_msg;
rclcpp::Clock ros_clock(RCL_ROS_TIME);
image_msg.header.stamp = ros_clock.now();
image_msg.header.frame_id = in->frame_id;
image_msg.encoding = sensor_msgs::image_encodings::RGB8;
image_msg.width = in->width;
image_msg.height = in->height;
image_msg.step = image_msg.width * 3;
int data_len = image_msg.step * image_msg.height;
image_msg.data.resize(data_len);
image_msg.is_bigendian = 0;
for(unsigned int i = 0; i < image_msg.height; i++)
{
int msg_idx = (image_msg.height - i - 1) * image_msg.step;
int buf_idx = i * image_msg.step;
for(unsigned int j = 0; j < image_msg.step; j++)
{
image_msg.data[msg_idx + j] = in->data[buf_idx + j];
}
}
publisherProxy->imageTransportPublisher.publish(image_msg);
#else
throw sim::exception("image_transport not available. please rebuild this plugin.");
#endif
}
void getTime(getTime_in *in, getTime_out *out)
{
rcl_clock_type_t t = RCL_ROS_TIME;
switch(in->clock_type)
{
case simros2_clock_ros: t = RCL_ROS_TIME; break;
case simros2_clock_system: t = RCL_SYSTEM_TIME; break;
case simros2_clock_steady: t = RCL_STEADY_TIME; break;
}
rclcpp::Clock ros_clock(t);
builtin_interfaces::msg::Time ros_now = ros_clock.now();
out->time.sec = ros_now.sec;
out->time.nanosec = ros_now.nanosec;
}
void getParamString(getParamString_in *in, getParamString_out *out)
{
out->exists = params_client->has_parameter(in->name);
if(out->exists)
{
auto ¶m = params_client->get_parameters({in->name}).front();
if(param.get_type() == rclcpp::PARAMETER_STRING)
out->value = param.get_value<rclcpp::PARAMETER_STRING>();
}
}
void getParamInt(getParamInt_in *in, getParamInt_out *out)
{
out->exists = params_client->has_parameter(in->name);
if(out->exists)
{
auto ¶m = params_client->get_parameters({in->name}).front();
if(param.get_type() == rclcpp::PARAMETER_INTEGER)
out->value = param.get_value<rclcpp::PARAMETER_INTEGER>();
}
}
void getParamDouble(getParamDouble_in *in, getParamDouble_out *out)
{
out->exists = params_client->has_parameter(in->name);
if(out->exists)
{
auto ¶m = params_client->get_parameters({in->name}).front();
if(param.get_type() == rclcpp::PARAMETER_DOUBLE)
out->value = param.get_value<rclcpp::PARAMETER_DOUBLE>();
}
}
void getParamBool(getParamBool_in *in, getParamBool_out *out)
{
out->exists = params_client->has_parameter(in->name);
if(out->exists)
{
auto ¶m = params_client->get_parameters({in->name}).front();
if(param.get_type() == rclcpp::PARAMETER_BOOL)
out->value = param.get_value<rclcpp::PARAMETER_BOOL>();
}
}
void setParamString(setParamString_in *in, setParamString_out *out)
{
params_client->set_parameters({rclcpp::Parameter(in->name, in->value)});
}
void setParamInt(setParamInt_in *in, setParamInt_out *out)
{
params_client->set_parameters({rclcpp::Parameter(in->name, in->value)});
}
void setParamDouble(setParamDouble_in *in, setParamDouble_out *out)
{
params_client->set_parameters({rclcpp::Parameter(in->name, in->value)});
}
void setParamBool(setParamBool_in *in, setParamBool_out *out)
{
params_client->set_parameters({rclcpp::Parameter(in->name, in->value)});
}
void hasParam(hasParam_in *in, hasParam_out *out)
{
out->exists = params_client->has_parameter(in->name);
}
void deleteParam(deleteParam_in *in, deleteParam_out *out)
{
if(params_client->has_parameter(in->name))
params_client->set_parameters({rclcpp::Parameter(in->name, rclcpp::ParameterValue())});
}
void createInterface(createInterface_in *in, createInterface_out *out)
{
if(0) {}
#include <if_new.cpp>
else
{
throw unsupported_type("interface", in->type);
}
}
void getInterfaceConstants(getInterfaceConstants_in *in, getInterfaceConstants_out *out)
{
if(0) {}
#include <if_const.cpp>
else
{
throw unsupported_type("interface", in->type);
}
}
void supportedInterfaces(supportedInterfaces_in *in, supportedInterfaces_out *out)
{
#include <if_list.cpp>
}
bool initialize()
{
rclcpp::init(0, nullptr);
auto node_name = sim::getNamedStringParam("ROS2Interface.nodeName");
node = rclcpp::Node::make_shared(node_name.value_or("sim_ros2_interface"));
tfbr = new tf2_ros::TransformBroadcaster(node);
#if image_transport_FOUND
imtr = new image_transport::ImageTransport(node);
#endif
params_client = std::make_shared<rclcpp::SyncParametersClient>(node);
while(!params_client->wait_for_service(1s))
{
if(!rclcpp::ok())
{
sim::addLog(sim_verbosity_errors, "Interrupted while waiting for parameters service");
return false;
}
sim::addLog(sim_verbosity_debug, "Parameters service not available. Waiting for it...");
}
return true;
}
void shutdown()
{
rclcpp::shutdown();
params_client = nullptr;
node = nullptr;
#if image_transport_FOUND
delete imtr;
#endif
delete tfbr;
}
private:
int previousStopSimulationRequestCounter = -1;
rclcpp::Node::SharedPtr node = nullptr;
rclcpp::SyncParametersClient::SharedPtr params_client = nullptr;
tf2_ros::TransformBroadcaster *tfbr = nullptr;
#if image_transport_FOUND
image_transport::ImageTransport *imtr = nullptr;
#endif
sim::Handles<SubscriptionProxy*> subscriptionHandles;
sim::Handles<PublisherProxy*> publisherHandles;
sim::Handles<ClientProxy*> clientHandles;
sim::Handles<ServiceProxy*> serviceHandles;
sim::Handles<ActionClientProxy*> actionClientHandles;
sim::Handles<ActionServerProxy*> actionServerHandles;
};
SIM_PLUGIN(Plugin)
#include "stubsPlusPlus.cpp"
