V-REP 3.2.1

ros_bubble_rob/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(ros_bubble_rob)
+
+find_package(catkin REQUIRED)
+find_package(catkin REQUIRED COMPONENTS std_msgs sensor_msgs image_transport vrep_common)
+
+include_directories(${catkin_INCLUDE_DIRS})
+set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
+set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
+
+add_executable(rosBubbleRob src/rosBubbleRob.cpp) 
+target_link_libraries(rosBubbleRob ${catkin_LIBRARIES})
+add_dependencies(rosBubbleRob vrep_common_generate_messages_cpp)

ros_bubble_rob/mainpage.dox

@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b ros_bubble_rob 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

ros_bubble_rob/package.xml

@@ -0,0 +1,17 @@
+<package>
+  <name>ros_bubble_rob</name>
+  <description>BubbleRob node</description>
+  <maintainer email="marc@coppeliarobotics.com">Marc</maintainer>
+  <version>3.1.2</version>
+  <author>Marc</author>
+  <license>GPLv3</license>
+  <url>http://www.coppeliarobotics.com</url>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>opencv2</build_depend>
+  <build_depend>vrep_common</build_depend>
+  <build_depend>image_transport</build_depend>
+  <run_depend>roscpp</run_depend>
+</package>

ros_bubble_rob/src/rosBubbleRob.cpp

@@ -0,0 +1,164 @@
+// Copyright 2006-2015 Coppelia Robotics GmbH. All rights reserved. 
+// marc@coppeliarobotics.com
+// www.coppeliarobotics.com
+// 
+// -------------------------------------------------------------------
+// THIS FILE IS DISTRIBUTED "AS IS", WITHOUT ANY EXPRESS OR IMPLIED
+// WARRANTY. THE USER WILL USE IT AT HIS/HER OWN RISK. THE ORIGINAL
+// AUTHORS AND COPPELIA ROBOTICS GMBH WILL NOT BE LIABLE FOR DATA LOSS,
+// DAMAGES, LOSS OF PROFITS OR ANY OTHER KIND OF LOSS WHILE USING OR
+// MISUSING THIS SOFTWARE.
+// 
+// You are free to use/modify/distribute this file for whatever purpose!
+// -------------------------------------------------------------------
+//
+// This file was automatically created for V-REP release V3.2.1 on May 3rd 2015
+
+#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"
+
+// Used API services:
+#include "vrep_common/simRosEnablePublisher.h"
+#include "vrep_common/simRosEnableSubscriber.h"
+
+// Global variables (modified by topic subscribers):
+bool simulationRunning=true;
+bool sensorTrigger=false;
+float simulationTime=0.0f;
+
+// Topic subscriber callbacks:
+void infoCallback(const vrep_common::VrepInfo::ConstPtr& info)
+{
+	simulationTime=info->simulationTime.data;
+	simulationRunning=(info->simulatorState.data&1)!=0;
+}
+
+void sensorCallback(const vrep_common::ProximitySensorData::ConstPtr& sens)
+{
+	// We don't care about the detected distance here, we just trigger!
+	sensorTrigger=true;
+}
+
+// Main code:
+int main(int argc,char* argv[])
+{
+	// The joint handles and proximity sensor handles are given in the argument list
+	// (when V-REP launches this executable, V-REP will also provide the argument list)
+	int leftMotorHandle;
+	int rightMotorHandle;
+	int sensorHandle;
+	if (argc>=4)
+	{
+		leftMotorHandle=atoi(argv[1]);
+		rightMotorHandle=atoi(argv[2]);
+		sensorHandle=atoi(argv[3]);
+	}
+	else
+	{
+		printf("Indicate following arguments: 'leftMotorHandle rightMotorHandle sensorHandle'!\n");
+		sleep(5000);
+		return 0;
+	}
+
+	// Create a ROS node. The name has a random component: 
+	int _argc = 0;
+	char** _argv = NULL;
+	struct timeval tv;
+	unsigned int timeVal=0;
+	if (gettimeofday(&tv,NULL)==0)
+		timeVal=(tv.tv_sec*1000+tv.tv_usec/1000)&0x00ffffff;
+	std::string nodeName("rosBubbleRob");
+	std::string randId(boost::lexical_cast<std::string>(timeVal+int(999999.0f*(rand()/(float)RAND_MAX))));
+	nodeName+=randId;		
+	ros::init(_argc,_argv,nodeName.c_str());
+
+	if(!ros::master::check())
+		return(0);
+
+	ros::NodeHandle node("~");	
+	printf("rosBubbleRob just started with node name %s\n",nodeName.c_str());
+
+	// 1. Let's subscribe to V-REP's info stream (that stream is the only one enabled by default,
+	// and the only one that can run while no simulation is running):
+	ros::Subscriber subInfo=node.subscribe("/vrep/info",1,infoCallback);
+
+	// 2. Request V-REP to launch a publisher for the BubbleRob's proximity sensor:
+	ros::ServiceClient client_enablePublisher=node.serviceClient<vrep_common::simRosEnablePublisher>("/vrep/simRosEnablePublisher");
+	vrep_common::simRosEnablePublisher srv_enablePublisher;
+	srv_enablePublisher.request.topicName="proxData"+randId; // the requested topic name
+	srv_enablePublisher.request.queueSize=1; // the requested publisher queue size (on V-REP side)
+	srv_enablePublisher.request.streamCmd=simros_strmcmd_read_proximity_sensor; // the requested publisher type
+	srv_enablePublisher.request.auxInt1=sensorHandle; // some additional information the publisher needs (what proximity sensor)
+    if ( client_enablePublisher.call(srv_enablePublisher)&&(srv_enablePublisher.response.effectiveTopicName.length()!=0) )
+	{ 	// ok, the service call was ok, and the publisher was succesfully started on V-REP side
+		// V-REP is now streaming the proximity sensor data!
+
+		// 3. Let's subscribe to that proximity sensor data:
+		std::string topicName("/vrep/");
+		topicName+=srv_enablePublisher.response.effectiveTopicName; // Make sure to use the returned topic name, not the requested one (can be same)
+		ros::Subscriber sub=node.subscribe(topicName.c_str(),1,sensorCallback);
+
+		// 4. Let's tell V-REP to subscribe to the motor speed topic (publisher to that topic will be created further down):
+		ros::ServiceClient client_enableSubscriber=node.serviceClient<vrep_common::simRosEnableSubscriber>("/vrep/simRosEnableSubscriber");
+		vrep_common::simRosEnableSubscriber srv_enableSubscriber;
+
+		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; // the subscriber type
+		if ( client_enableSubscriber.call(srv_enableSubscriber)&&(srv_enableSubscriber.response.subscriberID!=-1) )
+		{	// ok, the service call was ok, and the subscriber was succesfully started on V-REP side
+			// V-REP is now listening to the desired motor joint states
+
+			// 5. Let's prepare a publisher of those motor speeds:
+			ros::Publisher motorSpeedPub=node.advertise<vrep_common::JointSetStateData>("wheels",1);
+
+			// 6. Finally we have the control loop:
+			float driveBackStartTime=-99.0f;
+			while (ros::ok()&&simulationRunning)
+			{ // this is the control loop (very simple, just as an example)
+				vrep_common::JointSetStateData motorSpeeds;
+				float desiredLeftMotorSpeed;
+				float desiredRightMotorSpeed;
+				if (simulationTime-driveBackStartTime<3.0f)
+				{ // driving backwards while slightly turning:
+					desiredLeftMotorSpeed=-3.1415*0.5;
+					desiredRightMotorSpeed=-3.1415*0.25;
+				}
+				else
+				{ // going forward:
+					desiredLeftMotorSpeed=3.1415;
+					desiredRightMotorSpeed=3.1415;
+					if (sensorTrigger)
+						driveBackStartTime=simulationTime; // We detected something, and start the backward mode
+					sensorTrigger=false;
+				}
+
+				// 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);
+				motorSpeeds.values.data.push_back(desiredLeftMotorSpeed);
+				motorSpeeds.values.data.push_back(desiredRightMotorSpeed);
+				motorSpeedPub.publish(motorSpeeds);
+
+				// handle ROS messages:
+				ros::spinOnce();
+
+				// sleep a bit:
+				usleep(5000);
+			}
+		}
+	}
+	ros::shutdown();
+	printf("rosBubbleRob just ended!\n");
+	return(0);
+}
+

vrep_common/CMakeLists.txt

@@ -0,0 +1,41 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(vrep_common)
+
+
+find_package(catkin REQUIRED COMPONENTS message_generation std_msgs sensor_msgs)
+
+include_directories(${catkin_INCLUDE_DIRS})
+
+add_message_files(FILES ForceSensorData.msg  JointSetStateData.msg  ObjectGroupData.msg  ProximitySensorData.msg  VisionSensorData.msg  VisionSensorDepthBuff.msg  VrepInfo.msg)
+
+add_service_files(FILES simRosAddStatusbarMessage.srv      simRosGetDialogInput.srv           simRosGetUIEventButton.srv            simRosSetJointState.srv
+simRosAppendStringSignal.srv       simRosGetDialogResult.srv          simRosGetUIHandle.srv                 simRosSetJointTargetPosition.srv
+simRosAuxiliaryConsoleClose.srv    simRosGetDistanceHandle.srv        simRosGetUISlider.srv                 simRosSetJointTargetVelocity.srv
+simRosAuxiliaryConsoleOpen.srv     simRosGetFloatingParameter.srv     simRosGetVisionSensorDepthBuffer.srv  simRosSetModelProperty.srv
+simRosAuxiliaryConsolePrint.srv    simRosGetFloatSignal.srv           simRosGetVisionSensorImage.srv        simRosSetObjectFloatParameter.srv
+simRosAuxiliaryConsoleShow.srv     simRosGetInfo.srv                  simRosLoadModel.srv                   simRosSetObjectIntParameter.srv
+simRosBreakForceSensor.srv         simRosGetIntegerParameter.srv      simRosLoadScene.srv                   simRosSetObjectParent.srv
+simRosClearFloatSignal.srv         simRosGetIntegerSignal.srv         simRosLoadUI.srv                      simRosSetObjectPose.srv
+simRosClearIntegerSignal.srv       simRosGetJointMatrix.srv           simRosPauseSimulation.srv             simRosSetObjectPosition.srv
+simRosClearStringSignal.srv        simRosGetJointState.srv            simRosReadCollision.srv               simRosSetObjectQuaternion.srv
+simRosCloseScene.srv               simRosGetLastErrors.srv            simRosReadDistance.srv                simRosSetObjectSelection.srv
+simRosCopyPasteObjects.srv         simRosGetModelProperty.srv         simRosReadForceSensor.srv             simRosSetSphericalJointMatrix.srv
+simRosCreateDummy.srv              simRosGetObjectChild.srv           simRosReadProximitySensor.srv         simRosSetStringSignal.srv
+simRosDisablePublisher.srv         simRosGetObjectFloatParameter.srv  simRosReadVisionSensor.srv            simRosSetUIButtonLabel.srv
+simRosDisableSubscriber.srv        simRosGetObjectGroupData.srv       simRosRemoveObject.srv                simRosSetUIButtonProperty.srv
+simRosDisplayDialog.srv            simRosGetObjectHandle.srv          simRosRemoveUI.srv                    simRosSetUISlider.srv
+simRosEnablePublisher.srv          simRosGetObjectIntParameter.srv    simRosSetArrayParameter.srv           simRosSetVisionSensorImage.srv
+simRosEnableSubscriber.srv         simRosGetObjectParent.srv          simRosSetBooleanParameter.srv         simRosStartSimulation.srv
+simRosEndDialog.srv                simRosGetObjectPose.srv            simRosSetFloatingParameter.srv        simRosStopSimulation.srv
+simRosEraseFile.srv                simRosGetObjectSelection.srv       simRosSetFloatSignal.srv              simRosSynchronous.srv
+simRosGetAndClearStringSignal.srv  simRosGetObjects.srv               simRosSetIntegerParameter.srv         simRosSynchronousTrigger.srv
+simRosGetArrayParameter.srv        simRosGetStringParameter.srv       simRosSetIntegerSignal.srv            simRosTransferFile.srv
+simRosGetBooleanParameter.srv      simRosGetStringSignal.srv          simRosSetJointForce.srv               simRosRemoveModel.srv
+simRosGetCollisionHandle.srv       simRosGetUIButtonProperty.srv      simRosSetJointPosition.srv)
+
+generate_messages(DEPENDENCIES std_msgs sensor_msgs)
+
+catkin_package(
+  LIBRARIES
+  CATKIN_DEPENDS
+)

vrep_common/mainpage.dox

@@ -0,0 +1,14 @@
+/**
+\mainpage
+\htmlinclude manifest.html
+
+\b vrep_common 
+
+<!-- 
+Provide an overview of your package.
+-->
+
+-->
+
+
+*/

vrep_common/msg/ForceSensorData.msg

@@ -0,0 +1,3 @@
+std_msgs/Int32 sensorState
+geometry_msgs/Vector3 force
+geometry_msgs/Vector3 torque

vrep_common/msg/JointSetStateData.msg

@@ -0,0 +1,3 @@
+std_msgs/Int32MultiArray handles
+std_msgs/UInt8MultiArray setModes
+std_msgs/Float32MultiArray values

vrep_common/msg/ObjectGroupData.msg

@@ -0,0 +1,4 @@
+std_msgs/Int32MultiArray handles
+std_msgs/Int32MultiArray intData
+std_msgs/Float32MultiArray floatData
+std_msgs/String stringData

vrep_common/msg/ProximitySensorData.msg

@@ -0,0 +1,3 @@
+geometry_msgs/Point32 detectedPoint
+std_msgs/Int32 detectedObject
+geometry_msgs/Point32 normalVector

vrep_common/msg/VisionSensorData.msg

@@ -0,0 +1,3 @@
+std_msgs/Int32 triggerState
+std_msgs/Float32MultiArray packetData
+std_msgs/Int32MultiArray packetSizes

vrep_common/msg/VisionSensorDepthBuff.msg

@@ -0,0 +1,3 @@
+std_msgs/Int32 x
+std_msgs/Int32 y
+std_msgs/Float32MultiArray data

vrep_common/msg/VrepInfo.msg

@@ -0,0 +1,4 @@
+std_msgs/Header headerInfo
+std_msgs/Int32 simulatorState
+std_msgs/Float32 simulationTime
+std_msgs/Float32 timeStep

vrep_common/package.xml

@@ -0,0 +1,17 @@
+<package>
+  <name>vrep_common</name>
+  <description>Common messages</description>
+  <maintainer email="marc@coppeliarobotics.com">Marc</maintainer>
+  <version>3.1.2</version>
+  <author>Marc</author>
+  <license>GPLv3</license>
+  <url>http://www.coppeliarobotics.com</url>
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>opencv2</build_depend>
+  <run_depend>roscpp</run_depend>
+</package>
+
+

vrep_common/srv/simRosAddStatusbarMessage.srv

@@ -0,0 +1,8 @@
+#
+# simInt simAddStatusbarMessage(const simChar* message)
+#
+
+string message
+---
+int32 result
+

vrep_common/srv/simRosAppendStringSignal.srv

@@ -0,0 +1,8 @@
+#
+# simInt simAppendStringSignal(const simChar* signalName,const simChar* signalValue,simInt stringLength)
+#
+
+string signalName
+string signalValue
+---
+int32 result

vrep_common/srv/simRosAuxiliaryConsoleClose.srv

@@ -0,0 +1,8 @@
+#
+# simInt simAuxiliaryConsoleClose(simInt consoleHandle)
+#
+
+int32 consoleHandle
+---
+int32 result
+

vrep_common/srv/simRosAuxiliaryConsoleOpen.srv

@@ -0,0 +1,14 @@
+#
+# simInt simAuxiliaryConsoleOpen(const simChar* title,simInt maxLines,simInt mode,simInt* position,simInt* size,simFloat* textColor,simFloat* backgroundColor)
+#
+
+string title
+int32 maxLines
+int32 mode
+int32[] position
+int32[] size
+float32[] textColor
+float32[] backgroundColor
+---
+int32 consoleHandle
+

vrep_common/srv/simRosAuxiliaryConsolePrint.srv

@@ -0,0 +1,9 @@
+#
+# simInt simAuxiliaryConsolePrint(simInt consoleHandle,const simChar* text)
+#
+
+int32 consoleHandle
+string text
+---
+int32 result
+

vrep_common/srv/simRosAuxiliaryConsoleShow.srv

@@ -0,0 +1,9 @@
+#
+# simInt simAuxiliaryConsoleShow(simInt consoleHandle,simBool showState)
+#
+
+int32 consoleHandle
+uint8 showState
+---
+int32 result
+

vrep_common/srv/simRosBreakForceSensor.srv

@@ -0,0 +1,8 @@
+#
+# simInt simBreakForceSensor(simInt objectHandle)
+#
+
+int32 objectHandle
+---
+int32 result
+

vrep_common/srv/simRosClearFloatSignal.srv

@@ -0,0 +1,8 @@
+#
+# simInt simClearFloatSignal(const simChar* signalName)
+#
+
+string signalName
+---
+int32 result
+