Go to the documentation of this file.00001 #include <ros/ros.h>
00002 #include <sensor_msgs/JointState.h>
00003 #include <std_msgs/Float64.h>
00004 #include <string>
00005
00006
00007 ros::Publisher pub_toRos;
00008
00009 ros::Publisher pub_toVrep;
00010
00011 std::string tf_prefix;
00012
00013 void stateCallback(const sensor_msgs::JointState::ConstPtr &msg) {
00014
00015 sensor_msgs::JointState msg_out;
00016 std_msgs::Header header;
00017 header.stamp = ros::Time::now();
00018 header.frame_id = tf_prefix + "/base/joint1";
00019
00020 msg_out.header = move(header);
00021 msg_out.effort = msg->effort;
00022 msg_out.position = msg->position;
00023 msg_out.velocity = msg->velocity;
00024
00025 pub_toRos.publish(move(msg_out));
00026 }
00027
00028 void velCallback(const std_msgs::Float64::ConstPtr &msg) {
00029
00030 pub_toVrep.publish(msg);
00031 }
00032
00033
00034
00035 int main(int argc, char **argv) {
00036
00037 ros::init(argc, argv, "rightMotor");
00038
00039 ros::NodeHandle n("~");
00040
00041
00042
00043 int vrep_no = 0;
00044
00045 std::string tf_prefix_path;
00046 if (n.searchParam("tf_prefix", tf_prefix_path))
00047 {
00048 n.getParam(tf_prefix_path, tf_prefix);
00049 }
00050
00051 std::string vrep_index_path;
00052 if (n.searchParam("vrep_index", vrep_index_path))
00053 {
00054 n.getParam(vrep_index_path, vrep_no);
00055 }
00056
00057
00058 ros::Subscriber sub_from_vrep = n.subscribe("/vrep/i" + std::to_string(vrep_no) +
00059 "_Pioneer_p3dx_rightMotor/getState",
00060 1000, stateCallback);
00061 pub_toRos = n.advertise<sensor_msgs::JointState>("getState", 1000);
00062 ROS_INFO("HAL(VREP): Right motor state msg publisher node initialized");
00063
00064
00065 pub_toVrep = n.advertise<std_msgs::Float64>(
00066 "/vrep/i" + std::to_string(vrep_no) + "_Pioneer_p3dx_rightMotor/setVel",
00067 1000);
00068 ros::Subscriber sub_from_ros =n.subscribe("setVel", 1000, velCallback);
00069 ROS_INFO("HAL(VREP): Right motor publisher to vrep: velocity msg initialized");
00070
00071
00072 ros::spin();
00073 return 0;
00074 }