Added a new class

src/robot_coppelia_ros_interface.cpp

@@ -0,0 +1,145 @@
+#include "robot_coppelia_ros_interface.h"
+#include "std_msgs/Float64MultiArray.h"
+#include "sensor_msgs/JointState.h"
+#include <sas_common/sas_common.h>
+#include <sas_conversions/sas_conversions.h>
+
+/**
+ * @brief robot_ros_interface
+ * @param nh
+ */
+RobotCoppeliaRosInterface::RobotCoppeliaRosInterface(const ros::NodeHandle& nh,
+                                                     const std::string& topic_prefix,
+                                                     const std::vector<std::string>& jointnames,
+                                                     const RobotCoppeliaROSConfiguration& configuration,
+                                                     std::atomic_bool* kill_this_node)
+    :nh_(nh),
+    topic_prefix_(topic_prefix),
+    jointnames_(jointnames),
+    kill_this_node_(kill_this_node),
+    configuration_(configuration),
+    clock_(configuration.thread_sampling_time_nsec)
+{
+
+    //subscriber_joint_state_ = nh_.subscribe(topic_prefix_+ "/get/joint_states", 1,
+    //                                        &RobotCoppeliaRosInterface::_joint_states_callback, this);
+
+    //publisher_target_joint_positions_ = nh_.advertise<std_msgs::Float64MultiArray>
+    //                                    (topic_prefix_ + "/set/target_joint_positions", 1);
+
+    subscriber_target_joint_positions_ = nh_.subscribe(topic_prefix_ + "/set/target_joint_positions", 1, &RobotCoppeliaRosInterface::_callback_target_joint_positions, this);
+    publisher_joint_states_ = nh_.advertise<sensor_msgs::JointState>(topic_prefix_+ "/get/joint_states", 1);
+
+    vi_ = std::make_shared<DQ_VrepInterface>();
+    vi_->connect(configuration_.ip, configuration_.port, 500, 10);
+    vi_->start_simulation();
+}
+
+RobotCoppeliaRosInterface::~RobotCoppeliaRosInterface()
+{
+    vi_->disconnect();
+}
+
+/**
+ * @brief get_joint_positions
+ * @return
+ */
+VectorXd RobotCoppeliaRosInterface::get_joint_positions() const
+{
+    return joint_positions_;
+}
+
+
+/**
+ * @brief set_joint_target_positions
+ * @param target_joint_positions
+ */
+void RobotCoppeliaRosInterface::set_joint_target_positions(const VectorXd &target_joint_positions)
+{
+
+    std_msgs::Float64MultiArray ros_msg;
+    ros_msg.data = sas::vectorxd_to_std_vector_double(target_joint_positions);
+    publisher_target_joint_positions_.publish(ros_msg);
+
+}
+
+void RobotCoppeliaRosInterface::set_joint_target_velocities(const VectorXd &target_joint_velocities)
+{
+
+}
+
+
+void RobotCoppeliaRosInterface::send_joint_states(const VectorXd &joint_positions, const VectorXd &joint_velocities, const VectorXd &joint_forces)
+{
+    sensor_msgs::JointState ros_msg;
+    if(joint_positions.size()>0)
+        ros_msg.position = sas::vectorxd_to_std_vector_double(joint_positions);
+    if(joint_velocities.size()>0)
+        ros_msg.velocity = sas::vectorxd_to_std_vector_double(joint_velocities);
+    if(joint_forces.size()>0)
+        ros_msg.effort = sas::vectorxd_to_std_vector_double(joint_forces);
+    publisher_joint_states_.publish(ros_msg);
+}
+
+int RobotCoppeliaRosInterface::control_loop()
+{
+    try{
+        clock_.init();
+        ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with CoppeliaSim...");
+
+
+        while(not _should_shutdown())
+        {
+            clock_.update_and_sleep();
+            ros::spinOnce();
+
+            VectorXd q = vi_->get_joint_positions(jointnames_);
+            joint_positions_ = q;
+            send_joint_states(q, VectorXd(), VectorXd());
+
+            if (target_joint_positions_.size()>0)
+            {
+                vi_->set_joint_target_positions(jointnames_, target_joint_positions_);
+            }
+
+            //auto q = get_joint_positions();
+            std::cout<<"q: "<<q.transpose()<<std::endl;
+            std::cout<<"target q: "<<target_joint_positions_.transpose()<<std::endl;
+
+
+
+            ros::spinOnce();
+        }
+    }
+    catch(const std::exception& e)
+    {
+        ROS_WARN_STREAM(ros::this_node::getName() + "::Error or exception caught::" << e.what());
+    }
+    catch(...)
+    {
+        ROS_WARN_STREAM(ros::this_node::getName() + "::Unexpected error or exception caught");
+    }
+
+    return 0;
+}
+
+
+
+/**
+ * @brief _joint_states_callback
+ * @param jointstate
+ */
+bool RobotCoppeliaRosInterface::_should_shutdown() const
+{
+    return (*kill_this_node_);
+}
+
+void RobotCoppeliaRosInterface::_joint_states_callback(const sensor_msgs::JointState::ConstPtr& jointstate)
+{
+    joint_positions_ = sas::std_vector_double_to_vectorxd(jointstate->position);
+}
+
+void RobotCoppeliaRosInterface::_callback_target_joint_positions(const std_msgs::Float64MultiArrayConstPtr &msg)
+{
+    target_joint_positions_ = sas::std_vector_double_to_vectorxd(msg->data);
+}