public BodyVelocityRequestMsg(JSONNode msg)
{
// parse message fields into class data
_header = new HeaderMsg(msg["header"]);
_goal = new GoalDescriptorMsg(msg["goal"]);
_twist = new TwistMsg(msg["twist"]);
_disable_axis = new Bool6AxisMsg(msg["disable_axis"]);
}
public BodyVelocityRequestMsg(HeaderMsg header,
GoalDescriptorMsg goal,
TwistMsg twist,
Bool6AxisMsg disable_axis
)
{
_header = header;
_goal = goal;
_twist = twist;
_disable_axis = disable_axis;
}
// Update is called once per frame in Unity
void Update()
{
GameObject OVRtouchcontroller = GameObject.Find("OVR_touch_controller");
// OVRtouchcontroller.
// fill in the body velocity request message:
// carregar un msg tipus body velocity request amb els continugts de Global Variables
double vx = Global_Variables.vel_linear_x;
double vy = Global_Variables.vel_linear_y;
double vz = Global_Variables.vel_linear_z;
double yaw = (double)Global_Variables.yaw;
// HeaderMsg header, GoalDescriptorMsg goal, TwistMsg twist, auv_msgs.Bool6AxisMsg disable_axis
// twist for the boby velocity request: linear + angular speeds
TwistMsg twist = new TwistMsg(new Vector3Msg(vx, vy, vz), new Vector3Msg(0.0, 0.0, Global_Variables.yaw));
Bool6AxisMsg dissable_axis = new Bool6AxisMsg(false, false, false, true, true, false); // dissable axis field for the body velocity request
string requester = "/teleoperation";
GoalDescriptorMsg goal = new GoalDescriptorMsg(requester, 0, 60); // id= 0, priority=60
// header: int seq, TimeMsg stamp, string frame_id
System.DateTime epochStart = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
double cur_time = ((System.DateTime.UtcNow - epochStart).TotalSeconds); // in seconds
//print("cur_time" + cur_time);
double decimalPart = cur_time % 1; // get the decimal part
int secs = (int)(cur_time - decimalPart); //take the integer part of the timestamp expressed in nanoseconds
//print("seconds" + secs);
int nsecs = (int)(decimalPart * 1000000000); // take the nanoseconds
//print("nsencs"+ nsecs);
TimeMsg stamp = new TimeMsg(secs, nsecs);
HeaderMsg header = new HeaderMsg(0, stamp, "/map"); //frame id=/map.
BodyVelocityRequestMsg msg = new BodyVelocityRequestMsg(header, goal, twist, dissable_axis); // create a clase of message to be published
//print("Body request" + twist);
if (Global_Variables.activate_control) //send velocity commands only if the Unity Control is active
// print("publish body vel. request:" + msg.ToString()); // print requested vel..
{
ros.Publish(RobotBodyVelocityRequest.GetMessageTopic(), msg); // descomentar !! -- 18/09/2017
}
//ros.Publish(RobotBodyVelocityRequest.GetMessageTopic(), twist); // descomentar !! -- 18/09/2017
if (Global_Variables.stop_motors) // dissable motors if required with the "A" button
{
ros.CallService("/control/disable_thrusters");
Global_Variables.stop_motors = false; // avoids to send the service all the time
}
if (Global_Variables.start_motors) // enable motors if required with button "B"
{
ros.CallService("/control/enable_thrusters");
Global_Variables.start_motors = false; // avoids to send the service all the time
}
ros.Render();
}