146 lines
4.6 KiB
C++
146 lines
4.6 KiB
C++
#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);
|
|
}
|