I tried to set wheels speed via joystick using ROS. The code is following
Code: Select all
#include <stdio.h>
#include <stdlib.h>
#include <ros/ros.h>
#include "../include/v_repConst.h"
// Used data structures:
#include "vrep_common/ProximitySensorData.h"
#include "vrep_common/VrepInfo.h"
#include "vrep_common/JointSetStateData.h"
#include "boost/bind.hpp"
//Tomasz
#include "geometry_msgs/Twist.h"
// Used API services:
#include "vrep_common/simRosEnablePublisher.h"
#include "vrep_common/simRosEnableSubscriber.h"
class Bubble
{
public:
Bubble();
ros::NodeHandle node;
std::string nodeName;
bool simulationRunning;
float simulationTime;
int scale;
int leftMotorHandle;
int rightMotorHandle;
geometry_msgs::Twist* pointerToTwist;
ros::Subscriber subJoy;
ros::Subscriber subInfo;
ros::Publisher motorSpeedPub;
ros::ServiceClient client_enableSubscriber;
vrep_common::simRosEnableSubscriber srv_enableSubscriber;
vrep_common::JointSetStateData motorSpeeds;
float desiredLeftMotorSpeed;
float desiredRightMotorSpeed;
void infoCallback(const vrep_common::VrepInfo::ConstPtr& info)
{
simulationTime=info->simulationTime.data;
simulationRunning=(info->simulatorState.data&1)!=0;
}
void joyCallback(const geometry_msgs::Twist& joyMsg)//, geometry_msgs::Twist* pointer)
{
//twist = joyMsg;
std::cout<<"here joyMsg.linear.x = "<<joyMsg.linear.x<<"\n";
pointerToTwist->angular.z = joyMsg.angular.z;
pointerToTwist->linear.x = joyMsg.linear.x;
std::cout<<"here pointerToTwist->linear.x= "<<pointerToTwist->linear.x<<"\n";
desiredLeftMotorSpeed = pointerToTwist->linear.x;//*(pointerToTwist->angular.z + 2); //3.1415*0.5;
desiredRightMotorSpeed = pointerToTwist->linear.x;//*(-pointerToTwist->angular.z + 2);//3.1415*0.25;
// publish the motor speeds:
motorSpeeds.handles.data.push_back(leftMotorHandle);
motorSpeeds.handles.data.push_back(rightMotorHandle);
motorSpeeds.setModes.data.push_back(2); // 2 is the speed mode
motorSpeeds.setModes.data.push_back(2);
std::cout<<"here desiredRightMotorSpeed= "<<desiredRightMotorSpeed<<"\n";
std::cout<<"here desiredRightMotorSpeed= "<<desiredRightMotorSpeed<<"\n";
motorSpeeds.values.data.push_back(desiredLeftMotorSpeed);
motorSpeeds.values.data.push_back(desiredRightMotorSpeed);
motorSpeedPub.publish(motorSpeeds);
}
private:
};
Bubble::Bubble()
{
leftMotorHandle=atoi("leftMotor");
rightMotorHandle=atoi("rightMotor");
nodeName = "rosBubbleRobTomasz";
simulationRunning=true;
simulationTime=0.0f;
scale = 320000;
pointerToTwist = new geometry_msgs::Twist;
subJoy=node.subscribe("/vrep/joy",1000, &Bubble::joyCallback, this);//boost::bind(joyCallback, _1, pointerToTwist));
subInfo=node.subscribe("/vrep/info",1, &Bubble::infoCallback, this);
motorSpeedPub = node.advertise<vrep_common::JointSetStateData>("rosBubbleRobTomasz/wheels",1);
client_enableSubscriber=node.serviceClient<vrep_common::simRosEnableSubscriber>("/vrep/simRosEnableSubscriber");
srv_enableSubscriber.request.topicName="/"+nodeName+"/wheels"; // the topic name
srv_enableSubscriber.request.queueSize=1; // the subscriber queue size (on V-REP side)
srv_enableSubscriber.request.streamCmd=simros_strmcmd_set_joint_state;
}
// Main code:
int main(int argc,char* argv[])
{
int _argc = 0;
char** _argv = NULL;
std::string nodeName("rosBubbleRobTomasz");
ros::init(_argc, _argv, nodeName.c_str());
Bubble myBubble;
if(!ros::master::check())
return(0);
printf("rosBubbleRobTomasz just started with node name %s\n", nodeName.c_str());
if ( myBubble.client_enableSubscriber.call(myBubble.srv_enableSubscriber)&&(myBubble.srv_enableSubscriber.response.subscriberID!=-1) )
{
while (ros::ok() && myBubble.simulationRunning)
{
ros::spinOnce();
usleep(5000);
}
}
ros::shutdown();
printf("rosBubbleRobTomasz just ended!\n");
return(0);
}
std::couts output correct joystick messages. The problem is in the following section:
Code: Select all
desiredLeftMotorSpeed = pointerToTwist->linear.x;//*(pointerToTwist->angular.z + 2); //3.1415*0.5;
desiredRightMotorSpeed = pointerToTwist->linear.x;//*(-pointerToTwist->angular.z + 2);//3.1415*0.25;
// publish the motor speeds:
motorSpeeds.handles.data.push_back(leftMotorHandle);
motorSpeeds.handles.data.push_back(rightMotorHandle);
motorSpeeds.setModes.data.push_back(2); // 2 is the speed mode
motorSpeeds.setModes.data.push_back(2);
std::cout<<"here desiredRightMotorSpeed= "<<desiredRightMotorSpeed<<"\n";
std::cout<<"here desiredRightMotorSpeed= "<<desiredRightMotorSpeed<<"\n";
motorSpeeds.values.data.push_back(desiredLeftMotorSpeed);
motorSpeeds.values.data.push_back(desiredRightMotorSpeed);
nodes and topics connections are fine,
echoing msgs flowing through topics are okey also
thanks for help