new public void Button_Red()
{
//Debug.Log ("Red button clickedasdfasdfasdf");
var ButtonObj = GameObject.Find("WorldObjects/Canvas/Button0");
var DropdownObj = GameObject.Find("WorldObjects/Canvas/Dropdown");
if (CustomRosBridge.isMoving)
{
CustomRosBridge.isMoving = false;
ButtonObj.GetComponentInChildren <Text>().text = "Stop";
//Stop navigation
TwistMsg msg = new TwistMsg(new Vector3Msg(0, 0, 0), new Vector3Msg(0, 0, 0));
if (CustomRosBridge.ros != null)
{
CustomRosBridge.ros.Publish(RobotVelPublisher.GetMessageTopic(), msg);
}
}
else
{
CustomRosBridge.isMoving = true;
ButtonObj.GetComponentInChildren <Text>().text = "Confirm";
//Start navigation
StringMsg msg = new StringMsg("\"" + DropdownObj.GetComponentInChildren <Label>().text + "\"");
if (CustomRosBridge.ros != null)
{
CustomRosBridge.ros.Publish(RobotDataPublisher.GetMessageTopic(), msg);
}
}
}
protected override void StartROS()
{
Debug.Log("Started ROS");
Debug.Log(_rosBridge);
_rosLocomotionDirect = new ROSLocomotionDirect(ROSAgent.AgentJob.Subscriber, _rosBridge, "/cmd_vel");
_rosLocomotionDirect.OnDataReceived += ReceivedLocomotionDirectUpdate;
_rosJoystick = new ROSGenericSubscriber <TwistMsg>(_rosBridge, "/teleop_velocity_smoother/raw_cmd_vel",
TwistMsg.GetMessageType(), (msg) => new TwistMsg(msg));
_rosJoystick.OnDataReceived += ReceivedJoystickUpdate;
//odometry subscriber is not needed for the virtual robot
//_rosOdometrySubscriber = new ROSGenericSubscriber<OdometryMsg>(_rosBridge, "/odom", OdometryMsg.GetMessageType(), (msg) => new OdometryMsg(msg));
//_rosOdometrySubscriber.OnDataReceived += ReceivedOdometryUpdate;
//odometry publisher
_rosOdometry = new ROSGenericPublisher(_rosBridge, "/odom", OdometryMsg.GetMessageType());
//robot_gps_pose publisher
_rosOdometryGPSPose = new ROSGenericPublisher(_rosBridge, "/robot_gps_pose", OdometryMsg.GetMessageType());
//odo_calib_pose publisher
_rosOdometryOverride = new ROSGenericPublisher(_rosBridge, "/odo_calib_pose", OdometryMsg.GetMessageType());
_transformUpdateCoroutine = StartCoroutine(SendTransformUpdate(_publishInterval));
_rosLocomotionWaypointState = new ROSLocomotionWaypointState(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint/state");
_rosLocomotionWaypoint = new ROSLocomotionWaypoint(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint");
_rosLocomotionLinear = new ROSGenericPublisher(_rosBridge, "/waypoint/max_linear_speed", Float32Msg.GetMessageType());
_rosLocomotionAngular = new ROSGenericPublisher(_rosBridge, "/waypoint/max_angular_speed", Float32Msg.GetMessageType());
_rosLocomotionControlParams = new ROSLocomotionControlParams(ROSAgent.AgentJob.Publisher, _rosBridge, "/waypoint/control_parameters");
_rosLogger = new ROSGenericPublisher(_rosBridge, "/debug_output", StringMsg.GetMessageType());
_rosLocomotionLinear.PublishData(new Float32Msg(RobotConfig.MaxLinearSpeed));
_rosLocomotionAngular.PublishData(new Float32Msg(RobotConfig.MaxAngularSpeed));
_rosLocomotionControlParams.PublishData(RobotConfig.LinearSpeedParameter, RobotConfig.RollSpeedParameter, RobotConfig.PitchSpeedParameter, RobotConfig.AngularSpeedParameter);
}
void Update()
{
ros.Render();
if (!_running)
{
return;
}
timer -= Time.deltaTime;
if (timer <= 0)
{
timer = pollRate;
PointMsg point = new PointMsg(1, 2, 3);
QuaternionMsg quat = new QuaternionMsg(1, 2, 3, 4);
PoseMsg pose = new PoseMsg(point, quat);
PoseWithCovarianceMsg posec = new PoseWithCovarianceMsg(pose);
Vector3Msg vec3 = new Vector3Msg(1, 2, 3);
TwistMsg twist = new TwistMsg(vec3, vec3);
TwistWithCovarianceMsg twistc = new TwistWithCovarianceMsg(twist, new double[36]);
HeaderMsg header = new HeaderMsg(1, new TimeMsg(1, 1), "0");
PoseStampedMsg ps = new PoseStampedMsg(header, pose);
PathMsg msg = new PathMsg(header, new PoseStampedMsg[] { ps, ps, ps });
BoolMsg boolmsg = new BoolMsg(true);
StringMsg str = new StringMsg("This is a test");
RegionOfInterestMsg roi = new RegionOfInterestMsg(0, 1, 2, 3, true);
CameraInfoMsg caminfo = new CameraInfoMsg(header, 100, 200, "plumb_bob", new double[5], new double[9], new double[9], new double[12], 14, 123, roi);
_genericPub.PublishData(caminfo);
}
}
// it is called every frame update
void Update()
{
// render the ros communication
ros.Render();
// get the player object position
double playerPosX = player.transform.position.x;
double playerPosY = player.transform.position.y;
double playerPosZ = player.transform.position.z;
// get the player position vector
Vector3Msg playerPositionVector = new Vector3Msg(playerPosX, playerPosY, playerPosZ);
// get the player rotation vector
Vector3Msg playerRotationVector = new Vector3Msg(0, 0, 0);
// get the twist message for the player pose
TwistMsg playerPoseMsg = new TwistMsg(playerPositionVector, playerRotationVector);
// publish the message
ros.Publish(BallPosePublisher.GetMessageTopic(), playerPoseMsg);
// get player speed
float playerSpeed = (float)BallSpeedSubscriber.playerSpeed;
// update player speed
playerController.speed = (playerSpeed == 0)? 1 : playerSpeed;
}
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 void RemoteControl(Vector3 linear, Vector3 angular)
{
if (lineOn)
{
TwistMsg msg = new TwistMsg(new Vector3Msg(linear.x, linear.y, linear.z), new Vector3Msg(angular.x, angular.y, angular.z));
ros.Publish(RobotTeleop.GetMessageTopic(), msg);
}
}
/// <summary>
/// Publish velocity to turtlesim through the ROSBridge
/// </summary>
/// <param name="linearInput"></param>
/// <param name="angularInput"></param>
public void SendVelocity(float linearInput, float angularInput)
{
Vector3Msg linearVelocity = new Vector3Msg(linearInput, 0f, 0f);
Vector3Msg angularVelocity = new Vector3Msg(0f, 0f, -angularInput);
TwistMsg msg = new TwistMsg(linearVelocity, angularVelocity);
ROSBridge.Instance.Publish(TurtleVelocityPublisher.GetMessageTopic(), msg);
}
public TwistWithCovarianceMsg(JSONNode msg)
{
_twist = new TwistMsg(msg["twist"]);
// Treat covariance
for (int i = 0; i < msg["covariance"].Count; i++)
{
_covariance[i] = double.Parse(msg["covariance"][i]);
}
}
public TwistWithCovarianceMsg(JSONNode msg)
{
_twist = new TwistMsg(msg["twist"]);
_covariance = new double[msg["covariance"].Count];
for (int i = 0; i < _covariance.Length; i++)
{
_covariance[i] = double.Parse(msg["covariance"][i], System.Globalization.CultureInfo.InvariantCulture);
}
}
public static void GUI(this TwistMsg message, string name = "")
{
if (name.Length > 0)
{
GUILayout.Label(name);
}
message.linear.GUI("Linear");
message.angular.GUI("Angular");
}
public override void Deserialize(JSONNode msg)
{
_twist = new TwistMsg(msg["twist"]);
// Treat covariance
for (int i = 0; i < msg["covariance"].Count; i++)
{
_covariance[i] = double.Parse(msg["covariance"][i]);
}
}
public BodyVelocityRequestMsg(HeaderMsg header,
GoalDescriptorMsg goal,
TwistMsg twist,
Bool6AxisMsg disable_axis
)
{
_header = header;
_goal = goal;
_twist = twist;
_disable_axis = disable_axis;
}
/// <param name="linear">Linear movement speed in meter/second</param>
/// <param name="angular">Angular rotational speed in meter/second</param>
public void PublishData(float linear, float angular)
{
if (_publisher == null)
{
return;
}
TwistMsg twist = new TwistMsg(new Vector3Msg(linear, 0, 0),
new Vector3Msg(0, 0, angular));
_publisher.PublishData(twist);
}
// 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();
}
public TwistWithCovarianceMsg(TwistMsg twist, double[] covariance)
{
_twist = twist;
if (covariance.Length == 36)
{
_covariance = covariance;
}
else
{
_covariance = new double[36];
}
}
void FixedUpdate()
{
if (!isLocalPlayer)
{
return;
}
command_topic = string.Concat(inputField.text, "/cmd_vel2");
if (!msg_flag)
{
return;
}
TwistMsg cmdvel = (TwistMsg)in_msg;
m_Car.Move((float)(-1 * cmdvel.GetAngular().GetZ()), (float)(cmdvel.GetLinear().GetX()), (float)(cmdvel.GetLinear().GetX()), 0f);
// m_Car.Move((float)(cmdvel.GetAngular().GetZ()),(float)0.1 ,(float)0.1, 0f);
}
// Update is called once per frame in Unity. We use the joystick or cursor keys to generate teleoperational commands
// that are sent to the ROS world, which drives the robot which ...
private void Update()
{
if (updateControl)
{
// Instantiates variables with keyboad input (Lines 44 - 62).
float _dx, _dy;
if (_useJoysticks)
{
_dx = Input.GetAxis("Joy0X");
_dy = Input.GetAxis("Joy0Y");
}
else
{
_dx = Input.GetAxis("Horizontal");
_dy = Input.GetAxis("Vertical");
}
// Multiplying _dy or _dx by a larger value, increases "speed".
// Linear is responsibile for forward and backward movment.
var linear = _dy * 3.0f;
//angular is responsible for rotaion.
var angular = -_dx * 2.0f;
// Create a ROS Twist message from the keyboard input. This input/twist message, creates the data that will in turn move the
// bot on the ground.
var msg = new TwistMsg(new Vector3Msg(linear, 0.0, 0.0),
new Vector3Msg(0.0, 0.0, angular));
///////\\\\\\\\\ Need to call func to get appropriate topic
// name for the correct selected bot. This is for testing of
// the 4 bot environment!!!!!!
// Publishes the TwistMsg values over to the /cmd_vel topic in ROS.
_ros.Publish("/cmd_vel", msg);
_ros.Publish(FinalTopic,
msg); /////////\\\\\\\\\\ this is for testing of the 4 bots
// environment!!!
_ros.Render();
}
}
public new static void CallBack(ROSBridgeMsg msg)
{
TwistMsg OdomData = msg as TwistMsg;
ROSManager.getInstance().setLinear(OdomData.GetLinear());
ROSManager.getInstance().setAngular(OdomData.GetAngular());
Debug.Log(OdomData.ToYAMLString());
//GeoPointMsg LocationGPS = msg as GeoPointMsg;
//ROSManager.getInstance().setLatitude(LocationGPS.GetLatitude());
//ROSManager.getInstance().setLongitude(LocationGPS.GetLongitude());
//ROSManager.getInstance().setAltitude(LocationGPS.GetAltitude());
//Debug.Log(LocationGPS.ToYAMLString());
//CompressedImageMsg imgMsg = msg as CompressedImageMsg;
//ROSManager.getInstance().getUBDCam().LoadImage(imgMsg.GetImage());
}
public ROSLocomotionDirect(AgentJob job, ROSBridgeWebSocketConnection rosConnection, string topicName)
{
if (job == AgentJob.Publisher)
{
_publisher = new ROSGenericPublisher(rosConnection, topicName, TwistMsg.GetMessageType());
rosConnection.AddPublisher(_publisher);
}
else if (job == AgentJob.Subscriber)
{
_subscriber = new ROSGenericSubscriber <TwistMsg>(rosConnection, topicName, TwistMsg.GetMessageType(), (msg) => new TwistMsg(msg));
_subscriber.OnDataReceived += (data) =>
{
if (OnDataReceived != null)
{
OnDataReceived(data);
}
};
rosConnection.AddSubscriber(_subscriber);
}
}
// Update is called once per frame in Unity. The Unity camera follows the robot (which is driven by
// the ROS environment. We also use the joystick or cursor keys to generate teleoperational commands
// that are sent to the ROS world, which drives the robot which ...
void Update()
{
float _dx, _dy;
int _button = 0;
if (_useJoysticks)
{
_dx = Input.GetAxis("Joy0X");
_dy = Input.GetAxis("Joy0Y");
}
else
{
_dx = Input.GetAxis("Horizontal");
_dy = Input.GetAxis("Vertical");
Debug.Log("no joysticks " + _dx + " " + _dy);
}
float linear = _dy * 0.5f;
float angular = -_dx * 0.2f;
TwistMsg msg = new TwistMsg(new Vector3Msg(0.1, 0.2, 0.3), new Vector3Msg(-0.1, -0.2, -0.3));
ros.Publish(RobotTeleop.GetMessageTopic(), msg);
if (Input.GetKeyDown(KeyCode.T))
{
if (lineOn)
{
ros.CallService("/turtle1/set_pen", "{\"off\": 1}");
}
else
{
ros.CallService("/turtle1/set_pen", "{\"off\": 0}");
}
lineOn = !lineOn;
}
ros.Render();
}
// Update is called once per frame in Unity. We use the joystick or cursor keys to generate teleoperational commands
// that are sent to the ROS world, which drives the robot which ...
private void Update()
{
if (_useJoysticks)
{
_angular = Input.GetAxis("Joy0X");
_linear = Input.GetAxis("Joy0Y");
}
else
{
_angular = Input.GetAxis("Horizontal");
_linear = Input.GetAxis("Vertical");
}
// Multiplying _dy or _dx by a larger value, increases "speed".
// Linear is responsibile for forward and backward movment.
_linear *= 1.0f;
//angular is responsible for rotation.
_angular = -_angular * 1.0f;
// Create a ROS Twist message from the keyboard input. This input/twist message, creates the data that will in turn move the
// bot on the ground.
var msg = new TwistMsg(new Vector3Msg(_linear, 0.0, 0.0),
new Vector3Msg(0.0, 0.0, _angular));
_topic = "";
ActiveToggle();
// Publishes the TwistMsg values over to the /cmd_vel topic in ROS.
//_ros.Publish("/cmd_vel", msg);
_ros.Publish(_topic,
msg); /////////\\\\\\\\\\ this is for testing of the 4 bots
// environment!!!
_ros.Render();
}
public void Send_message()
{
float _dx = 0.1f, _dy = 0.2f;
float linear = _dy * 0.5f;
float angular = -_dx * 0.2f;
TwistMsg msg = new TwistMsg(new Vector3Msg(linear, 10, 10), new Vector3Msg(-10, 40, angular));
ros.Publish(Turtle1Teleop.GetMessageTopic(), msg);
Debug.Log("Message sent to ROS");
if (Input.GetKeyDown(KeyCode.T))
{
if (lineOn)
{
ros.CallService("/turtle1/set_pen", "{\"off\": 1}");
}
else
{
ros.CallService("/turtle1/set_pen", "{\"off\": 0}");
}
lineOn = !lineOn;
}
ros.Render();
}
private void ReceivedJoystickUpdate(ROSBridgeMsg data)
{
_joystickDataToConsume = (TwistMsg)data;
_hasJoystickDataToConsume = true;
}
public static string ToYAMLString(TwistMsg msg)
{
return(msg.ToYAMLString());
}
private void PublishVelocity(TwistMsg twist)
{
_rosLocomotionDirect.PublishData((float)twist._linear._x, (float)twist._angular._z);
}
void Awake()
{
vel = new TwistMsg(
new Vector3Msg(0, 0, 0),
new Vector3Msg(0, 0, 0));
}
public TwistWithCovarianceMsg(TwistMsg twist)
{
_twist = twist;
_covariance = new double[36];
}
public new static string GetMessageType()
{
return(TwistMsg.GetMessageType());
}
public TwistWithCovarianceMsg(TwistMsg twist, double[] covariance)
{
_twist = twist;
_covariance = covariance;
}
private void ReceivedLocomotionDirectUpdate(ROSBridgeMsg data)
{
_locomotionDirectDataToConsume = (TwistMsg)data;
_hasLocomotionDirectDataToConsume = true;
}
