Skip to content

Ros2 magician binding wip #99

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
torydebra wants to merge 5 commits into
base: ros2-force_limits-magician
Choose a base branch
from
Open

Ros2 magician binding wip #99

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
5 commits
Select commit Hold shift + click to select a range
6ff564a
not working
torydebra May 8, 2025
d2984dd
devs
torydebra May 8, 2025
0e0da7e
RosImpl port to ros2, still to check something
torydebra May 9, 2025
5e0d0c6
ros2 port for python completed?
torydebra May 13, 2025
e4338a3
comply with humble issues
torydebra May 13, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
41 changes: 21 additions & 20 deletions bindings/python/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,49 +7,50 @@ if(${pybind11_FOUND})

message(STATUS "compiling python bindings")

# get python install path
message(STATUS "Found python (${Python3_EXECUTABLE}) will compile python bindings")

# get python install path
execute_process(
COMMAND ${PYTHON_EXECUTABLE} -c "import sys; version = sys.version_info[:3]; print('{0}.{1}'.format(*version))"
OUTPUT_VARIABLE PYTHON_VERSION_MIN
COMMAND ${Python3_EXECUTABLE} -c "import site, os; print(os.path.relpath(site.USER_SITE, site.USER_BASE))"
OUTPUT_VARIABLE PYTHON_SITE
OUTPUT_STRIP_TRAILING_WHITESPACE)

set(PYTHON_SITE "lib/python${PYTHON_VERSION_MIN}/site-packages")

message(STATUS "Python install dir: ${PYTHON_SITE}")

include_directories(${CMAKE_CURRENT_SOURCE_DIR})
pybind11_add_module(pyest pyForceEstimation.cpp)
target_link_libraries(pyest PUBLIC estimation_utils::payload_estimation)

install(TARGETS pyest
DESTINATION ${PYTHON_SITE})

# include_directories(${CMAKE_CURRENT_SOURCE_DIR})
# pybind11_add_module(pyci pyRosImpl.cpp)
# target_link_libraries(pyci PRIVATE CartesianInterface)
DESTINATION ${PYTHON_SITE}/cartesian_interface)

# install(TARGETS pyci
# DESTINATION ${PYTHON_SITE})

# pybind11_add_module(roscpp_utils pyRosInit.cpp)
# target_link_libraries(roscpp_utils PRIVATE ${catkin_LIBRARIES})

# install(TARGETS roscpp_utils
# DESTINATION ${PYTHON_SITE})
include_directories(${CMAKE_CURRENT_SOURCE_DIR})
pybind11_add_module(pyci pyRosImpl.cpp)
target_link_libraries(pyci PRIVATE CartesianInterface CartesianInterfaceRos)

install(TARGETS pyci
DESTINATION ${PYTHON_SITE}/cartesian_interface)


pybind11_add_module(roscpp_utils pyRosInit.cpp)
#target_link_libraries(roscpp_utils PRIVATE ${catkin_LIBRARIES})
target_link_libraries(roscpp_utils PRIVATE rclcpp::rclcpp)

install(TARGETS roscpp_utils
DESTINATION ${PYTHON_SITE}/cartesian_interface)

pybind11_add_module(impedance pyImpedance.cpp)
target_link_libraries(impedance PRIVATE CartesianInterfaceCore)

install(TARGETS impedance
DESTINATION ${PYTHON_SITE})
DESTINATION ${PYTHON_SITE}/cartesian_interface)

install(FILES pyci_all.py
DESTINATION ${PYTHON_SITE})
DESTINATION ${PYTHON_SITE}/cartesian_interface)

install(PROGRAMS interactive_client.py
DESTINATION ${PYTHON_SITE})
DESTINATION ${PYTHON_SITE}/cartesian_interface)

else()
message(STATUS "pybind not found")
Expand Down
32 changes: 16 additions & 16 deletions bindings/python/pyRosImpl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,7 +14,7 @@
#include "problem/Com.h"
#include "../../examples/plugin/angular_mom/src/AngularMomentum.h"

#include <cartesian_interface/utils/RobotStatePublisher.h>
#include <cartesian_interface/ros/utils/RobotStatePublisher.h>

PYBIND11_MODULE(pyci, m) {

Expand Down Expand Up @@ -138,11 +138,11 @@ PYBIND11_MODULE(pyci, m) {

py::class_<ClientApi::PosturalRos,
PosturalTask,
ClientApi::PosturalRos::Ptr>(m, "PosturalRos", py::multiple_inheritance())
ClientApi::PosturalRos::Ptr>(m, "PosturalRos", py::multiple_inheritance());

py::class_<ClientApi::AngularMomentumRos,
AngularMomentum,
ClientApi::AngularMomentumRos::Ptr>(m, "AngularMomentumRos", py::multiple_inheritance());
// py::class_<ClientApi::AngularMomentumRos,
// AngularMomentum,
// ClientApi::AngularMomentumRos::Ptr>(m, "AngularMomentumRos", py::multiple_inheritance());

py::class_<CartesianTaskImpl,
CartesianTask,
Expand All @@ -156,9 +156,9 @@ PYBIND11_MODULE(pyci, m) {
PosturalTask,
PosturalTaskImpl::Ptr>(m, "PosturalTaskImpl", py::multiple_inheritance());

py::class_<AngularMomentumImpl,
AngularMomentum,
AngularMomentumImpl::Ptr>(m, "AngularMomentumImpl", py::multiple_inheritance());
// py::class_<AngularMomentumImpl,
// AngularMomentum,
// AngularMomentumImpl::Ptr>(m, "AngularMomentumImpl", py::multiple_inheritance());

py::class_<CartesianInterfaceImpl,
CartesianInterfaceImpl::Ptr>(m, "CartesianInterface")
Expand Down Expand Up @@ -235,14 +235,14 @@ PYBIND11_MODULE(pyci, m) {


/* Robot state pub util */
py::class_<Utils::RobotStatePublisher>(m, "RobotStatePublisher")
.def(py::init<XBot::ModelInterface::ConstPtr>())
.def("publishTransforms",
[](Utils::RobotStatePublisher& r, std::string pref)
{
r.publishTransforms(ros::Time::now(), pref);
})
;
// py::class_<Utils::RobotStatePublisher>(m, "RobotStatePublisher")
// .def(py::init<XBot::ModelInterface::ConstPtr>())
// .def("publishTransforms",
// [](Utils::RobotStatePublisher& r, std::string pref)
// {
// r.publishTransforms(ros::Time::now(), pref);
// })
// ;


}
Expand Down
29 changes: 17 additions & 12 deletions bindings/python/pyRosInit.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,14 +1,16 @@
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include <ros/ros.h>
#include <rclcpp/rclcpp.hpp>
#include <list>

bool init(std::string name, std::list<std::string> args)
{
if(ros::ok())
if(rclcpp::ok())
{
ROS_ERROR("Ros node already initialized with name %s",
ros::this_node::getName().c_str());
//ROS_ERROR("Ros node already initialized with name %s",
// ros::this_node::getName().c_str());
RCLCPP_ERROR(rclcpp::get_logger("rclcpp"), "Ros node already initialized");

return false;
}

Expand All @@ -24,22 +26,25 @@ bool init(std::string name, std::list<std::string> args)

name += "_cpp";

ros::init(argc, argv, name, ros::init_options::NoSigintHandler);

ROS_INFO("Initialized roscpp under namespace %s with name %s",
ros::this_node::getNamespace().c_str(),
ros::this_node::getName().c_str()
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared(name);

RCLCPP_INFO(node->get_logger(),
"Initialized roscpp under namespace %s with name %s",
node->get_namespace(),
node->get_name()
);

return true;
}

bool shutdown()
{
if(ros::ok())
if(rclcpp::ok())
{
ROS_INFO("Shutting down ros node");
ros::shutdown();
RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Shutting down ros node");

rclcpp::shutdown();
return true;
}

Expand Down
22 changes: 13 additions & 9 deletions include/cartesian_interface/ros/RosClient.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -4,25 +4,28 @@
#include <string>
#include <vector>

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>

#include <cartesian_interface/CartesianInterfaceImpl.h>

#include <cartesian_interface_ros/srv/load_controller.hpp>

namespace XBot { namespace Cartesian {

struct RosInitializer
{
RosInitializer(std::string ns);

ros::NodeHandle& nh();
rclcpp::Node::SharedPtr node();
void callAvailable();

private:

ros::CallbackQueue _queue;
std::unique_ptr<ros::NodeHandle> _nh;
rclcpp::CallbackGroup::SharedPtr _ros2_cbg;
rclcpp::Node::SharedPtr _node;
std::shared_ptr<rclcpp::executors::SingleThreadedExecutor> _exec;
};

class RosClient : public CartesianInterfaceImpl,
Expand Down Expand Up @@ -77,12 +80,13 @@ class RosClient : public CartesianInterfaceImpl,
const std::string& target_frame,
Eigen::Affine3d& t_T_s);

std::string _ns;

private:

tf::TransformListener _listener;
ros::ServiceClient _load_ctrl_srv;

std::shared_ptr<tf2_ros::TransformListener> _listener;
std::unique_ptr<tf2_ros::Buffer> _tf_buffer;
rclcpp::Client<cartesian_interface_ros::srv::LoadController>::SharedPtr _load_ctrl_srv;

};

Expand Down
Loading