コード例 #1
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setJoySensorCallback(const sensor_msgs::Joy::ConstPtr& joyPacket)
{
	if (_handleGeneralCallback_before())
	{
		if((auxInt1>=0)&&(auxInt2>=0)&&(auxInt2+auxInt1>0))
		{
			float floatPacket[auxInt1 + auxInt2];
			for(unsigned int i=0;i<(unsigned int)auxInt1;i++)
			{
				if (i<joyPacket->axes.size())
					floatPacket[i] = joyPacket->axes[i];
				else
					floatPacket[i] = 0.0f;
			}
			for(unsigned int j=0;j<(unsigned int)auxInt2;j++)
			{
				if (j<joyPacket->buttons.size())
					floatPacket[j+auxInt1] = (float)joyPacket->buttons[j];
				else
					floatPacket[j+auxInt1] = 0.0f;
			}

			simSetStringSignal(auxStr.c_str(),(simChar*)floatPacket,4*(auxInt1+auxInt2));
		}
		else
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #2
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setVisionSensorImageCallback(const sensor_msgs::Image::ConstPtr& image)
{
	if (_handleGeneralCallback_before())
	{
		int resol[2];
		int result=simGetVisionSensorResolution(auxInt1,resol);
		if (result!=-1)
		{
			if ((image->encoding==sensor_msgs::image_encodings::RGB8)&&(image->step==image->width*3)&&(int(image->height)==resol[1])&&(int(image->width)==resol[0]))
			{
				float* image_buff=new float[3*resol[0]*resol[1]];
				for(unsigned int i=0;i<image->height;i++)
				{
					int msg_idx=(image->height-i-1)*image->step;
					int buf_idx=i*image->step;
					for(unsigned int j=0;j<image->step;j++)
						image_buff[buf_idx+j]=float(image->data[msg_idx+j])/255.0f;
				}
				simSetVisionSensorImage(auxInt1,image_buff);
				delete[] image_buff;
			}
		}
		else
			shutDownImageSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #3
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setIntegerSignalCallback(const std_msgs::Int32::ConstPtr& sig)
{
	if (_handleGeneralCallback_before())
	{
		simSetIntegerSignal(auxStr.c_str(),sig->data);
		_handleGeneralCallback_after();
	}
}
コード例 #4
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setStringSignalCallback(const std_msgs::String::ConstPtr& sig)
{
	if (_handleGeneralCallback_before())
	{
		simSetStringSignal(auxStr.c_str(),&sig->data[0],sig->data.length());
		_handleGeneralCallback_after();
	}
}
コード例 #5
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setFloatingParameterCallback(const std_msgs::Float32::ConstPtr& param)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetFloatingParameter(auxInt1,param->data)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #6
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setObjectIntParameterCallback(const std_msgs::Int32::ConstPtr& param)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetObjectIntParameter(auxInt1,auxInt2,param->data)<=0)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #7
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setUIButtonPropertyCallback(const std_msgs::Int32::ConstPtr& prop)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetUIButtonProperty(auxInt1,auxInt2,prop->data)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #8
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setJointForceCallback(const std_msgs::Float64::ConstPtr& force)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetJointForce(auxInt1,(float)force->data)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #9
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setUISlider(const std_msgs::Int32::ConstPtr& pos)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetUISlider(auxInt1,auxInt2,pos->data)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #10
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::addStatusbarMessageCallback(const std_msgs::String::ConstPtr& msg)
{
	if (_handleGeneralCallback_before())
	{
		if (simAddStatusbarMessage(msg->data.c_str())==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #11
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setJointTargetVelocityCallback(const std_msgs::Float64::ConstPtr& vel)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetJointTargetVelocity(auxInt1,(float)vel->data)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #12
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setJointTargetPositionCallback(const std_msgs::Float64::ConstPtr& pos)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetJointTargetPosition(auxInt1,(float)pos->data)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #13
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setUIButtonLabelCallback(const std_msgs::String::ConstPtr& label)
{
	if (_handleGeneralCallback_before())
	{
		if (simSetUIButtonLabel(auxInt1,auxInt2,label->data.c_str(),NULL)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #14
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setObjectQuaternionCallback(const geometry_msgs::Quaternion::ConstPtr& quaternion)
{
	if (_handleGeneralCallback_before())
	{
		float v[4]={(float)quaternion->x,(float)quaternion->y,(float)quaternion->z,(float)quaternion->w};
		if (simSetObjectQuaternion(auxInt1,auxInt2,v)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #15
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setTwistCommandCallback(const geometry_msgs::Twist::ConstPtr& vel)
{
	if (_handleGeneralCallback_before())
	{
		simFloat command[6] = {vel->linear.x,vel->linear.y,vel->linear.z,vel->angular.x,vel->angular.y,vel->angular.z};
		if (simSetStringSignal(auxStr.c_str(),(simChar*)command,6*sizeof(simFloat))==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #16
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setObjectPositionCallback(const geometry_msgs::Point::ConstPtr& pos)
{
	if (_handleGeneralCallback_before())
	{
		float v[3]={(float)pos->x,(float)pos->y,(float)pos->z};
		if (simSetObjectPosition(auxInt1,auxInt2,v)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #17
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setTracksVelCommandCallback(const nifti_robot_driver_msgs::Tracks::ConstPtr& vel)
{
	if (_handleGeneralCallback_before())
	{
		simFloat command[2] = {vel->left,vel->right};
		if (simSetStringSignal(auxStr.c_str(),(simChar*)command,2*sizeof(simFloat))==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #18
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setArrayParameterCallback(const geometry_msgs::Point32::ConstPtr& param)
{
	if (_handleGeneralCallback_before())
	{
		float v[3]={param->x,param->y,param->z};
		if (simSetArrayParameter(auxInt1,(void*)v)==-1)
			shutDownGeneralSubscriber();
		_handleGeneralCallback_after();
	}
}
コード例 #19
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::clearStringSignalCallback(const std_msgs::UInt8::ConstPtr& options)
{
	if (_handleGeneralCallback_before())
	{
		if (options->data==0)
			simClearStringSignal(auxStr.c_str());
		else
			simClearStringSignal(NULL);
		_handleGeneralCallback_after();
	}
}
コード例 #20
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setObjectSelectionCallback(const std_msgs::Int32MultiArray::ConstPtr& objHandles)
{
	if (_handleGeneralCallback_before())
	{
		simRemoveObjectFromSelection(sim_handle_all,0);
		int cnt=objHandles->data.size();
		for (int i=0;i<cnt;i++)
			simAddObjectToSelection(sim_handle_single,objHandles->data[i]);
		_handleGeneralCallback_after();
	}
}
コード例 #21
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setObjectPoseCallback(const geometry_msgs::PoseStamped::ConstPtr& pose)
{
	if (_handleGeneralCallback_before())
	{
		float p[3]={(float)pose->pose.position.x,(float)pose->pose.position.y,(float)pose->pose.position.z};
		float q[4]={(float)pose->pose.orientation.x,(float)pose->pose.orientation.y,(float)pose->pose.orientation.z,(float)pose->pose.orientation.w};
		if (simSetObjectPosition(auxInt1,auxInt2,p)==-1)
			shutDownGeneralSubscriber();
		else
			simSetObjectQuaternion(auxInt1,auxInt2,q);
		_handleGeneralCallback_after();
	}
}
コード例 #22
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::appendStringSignalCallback(const std_msgs::String::ConstPtr& sig)
{
	if (_handleGeneralCallback_before())
	{
		std::string theNewString;
		int stringLength;
		char* stringSignal=simGetStringSignal(auxStr.c_str(),&stringLength);
		if (stringSignal!=NULL)
		{
			theNewString=std::string(stringSignal,stringLength);
			simReleaseBuffer(stringSignal);
		}
		theNewString+=std::string(&sig->data[0],sig->data.length());
		simSetStringSignal(auxStr.c_str(),theNewString.c_str(),theNewString.length());
		_handleGeneralCallback_after();
	}
}
コード例 #23
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::auxiliaryConsolePrintCallback(const std_msgs::String::ConstPtr& txt)
{
	if (_handleGeneralCallback_before())
	{
		if (txt->data.length()==0)
		{
			if (simAuxiliaryConsolePrint(auxInt1,NULL)<=0)
				shutDownGeneralSubscriber();
		}
		else
		{
			if (simAuxiliaryConsolePrint(auxInt1,txt->data.c_str())<=0)
				shutDownGeneralSubscriber();
		}
		_handleGeneralCallback_after();
	}
}
コード例 #24
0
ファイル: vrepSubscriber.cpp プロジェクト: Johan944/Need4Stek
void CSubscriberData::callScriptFunctionCallback(const vrep_common::ScriptFunctionCallData::ConstPtr& data)
{
	if (_handleGeneralCallback_before())
	{
		int options=auxInt1;

		int inStringCnt=0;
		std::string inStrings(data->stringData.data.begin(),data->stringData.data.end());

		if (inStrings.size()>0)
		{
			if (inStrings[inStrings.size()-1]!='\0')
				inStrings+='\0';
			for (size_t i=0;i<inStrings.size();i++)
			{
				if (inStrings[i]=='\0')
					inStringCnt++;
			}
		}
		std::vector<std::string> inString;
		int off=0;
		for (int i=0;i<inStringCnt;i++)
		{
			inString.push_back(inStrings.c_str()+off);
			off+=strlen(inStrings.c_str()+off)+1;
		}
		
		int inBufferSize=data->bufferData.data.size();

		CScriptFunctionData D;
		D.pushOutData_scriptFunctionCall(CScriptFunctionDataItem(data->intData.data));
		D.pushOutData_scriptFunctionCall(CScriptFunctionDataItem(data->floatData.data));
		D.pushOutData_scriptFunctionCall(CScriptFunctionDataItem(inString));
		D.pushOutData_scriptFunctionCall(CScriptFunctionDataItem(data->bufferData.data.c_str(),inBufferSize));
        int stackHandle=simCreateStack();
		D.writeDataToStack_scriptFunctionCall(stackHandle);
		simCallScriptFunctionEx(options,auxStr.c_str(),stackHandle);
        simReleaseStack(stackHandle);
		
		_handleGeneralCallback_after();
	}
}
コード例 #25
0
ファイル: vrepSubscriber.cpp プロジェクト: mario-gianni/ros
void CSubscriberData::setJointStateCallback(const vrep_common::JointSetStateData::ConstPtr& data)
{
	if (_handleGeneralCallback_before())
	{
		if ( (data->handles.data.size()>0)&&(data->handles.data.size()==data->setModes.data.size())&&(data->handles.data.size()==data->values.data.size()) )
		{
			for (unsigned int i=0;i<data->handles.data.size();i++)
			{
				int handle=data->handles.data[i];
				unsigned char setMode=data->setModes.data[i];
				float val=data->values.data[i];
				if (setMode==0)
					simSetJointPosition(handle,val);
				if (setMode==1)
					simSetJointTargetPosition(handle,val);
				if (setMode==2)
					simSetJointTargetVelocity(handle,val);
				if (setMode==3)
					simSetJointForce(handle,val);
			}
		}
		_handleGeneralCallback_after();
	}
}