ROS tutorial + joystick

[Tomaszz]

Hi all,

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);

it seems like desiredRightMotorSpeed and desiredLeftMotorSpeed don't change their values? the ros code is okey i guess, I think i miss sth in vrep API code.

nodes and topics connections are fine,
echoing msgs flowing through topics are okey also

thanks for help