public void Publish()
{
MNiryoMoveitJoints sourceDestinationMessage = new MNiryoMoveitJoints();
sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target;
sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target;
sourceDestinationMessage.joint_02 = jointArticulationBodies[2].xDrive.target;
sourceDestinationMessage.joint_03 = jointArticulationBodies[3].xDrive.target;
sourceDestinationMessage.joint_04 = jointArticulationBodies[4].xDrive.target;
sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target;
// Pick Pose
sourceDestinationMessage.pick_pose = new MPose
{
position = target.transform.position.To <FLU>(),
orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
};
// Place Pose
sourceDestinationMessage.place_pose = new MPose
{
position = targetPlacement.transform.position.To <FLU>(),
orientation = pickOrientation.To <FLU>()
};
// Finally send the message to server_endpoint.py running in ROS
ros.Send(topicName, sourceDestinationMessage);
}
void parseXYZ(ROSConnection rs, string filePath)
{
string[] lines = File.ReadAllLines(filePath);
double[] X = new double[lines.Length];
double[] Y = new double[lines.Length];
double[] Z = new double[lines.Length];
int i = 0;
foreach (string line in lines)
{
var words = line.Split(' ');
X[i] = (double.Parse(words[0]) / 1000);
Y[i] = (double.Parse(words[1]) / 1000);
Z[i] = (double.Parse(words[2]) / 1000);
i++;
}
RosMessageTypes.ROS.XYZcloud Cloud = new RosMessageTypes.ROS.XYZcloud();
Cloud.X = X;
Cloud.Y = Y;
Cloud.Z = Z;
if (DisplayPointCloud == true)
{
ApplyToParticleSystem(X, Y, Z);
}
else
{
var ps = GetComponent <ParticleSystem>();
ps.Stop();
}
rs.Send("/unity/XYZCloud", Cloud);
}
// ################################# Getters ############################################
// void getPosOri(ArticulationBody link)
// {
// Transform data = link.transform;
// // //print(data.eulerAngles);
// // print(data.position);
// return
// }
// void getEEinfo()
// {
// }
// ################################# Publishers #################################
void publishStates()
{
timeElapsed += Time.deltaTime;
if (timeElapsed > publishMessageFrequency)
{
Transform grasp_target = articulationChain[ee_index].transform;
Transform ee_info = articulationChain[ee_index - 1].transform;
PosRot eePosRot = new PosRot(
grasp_target.position.x,
grasp_target.position.y,
grasp_target.position.z,
ee_info.rotation.x,
ee_info.rotation.y,
ee_info.rotation.z,
ee_info.rotation.w
);
print(ee_info.rotation);
print(ee_info.eulerAngles);
print(grasp_target.position);
// Finally send the message to server_endpoint.py running in ROS
ros.Send(topicName, eePosRot);
timeElapsed = 0;
}
}
/// <summary>
/// Publish the robot's current joint configuration, the target object's
/// position and rotation, and the target placement for the target object's
/// position and rotation.
///
/// Includes conversion from Unity coordinates to ROS coordinates, Forward Left Up
/// </summary>
public void PublishJoints()
{
MoverServiceRequest request = new MoverServiceRequest
{
joints_input =
{
joint_00 = jointArticulationBodies[0].xDrive.target,
joint_01 = jointArticulationBodies[1].xDrive.target,
joint_02 = jointArticulationBodies[2].xDrive.target,
joint_03 = jointArticulationBodies[3].xDrive.target,
joint_04 = jointArticulationBodies[4].xDrive.target,
joint_05 = jointArticulationBodies[5].xDrive.target
},
pick_pose = new Pose
{
position = (target.transform.position + PICK_POSE_OFFSET).To <FLU>(),
// The hardcoded x/z angles assure that the gripper is always positioned above the target cube before grasping.
orientation = Quaternion.Euler(90, target.transform.eulerAngles.y, 0).To <FLU>()
},
place_pose = new Pose
{
position = (targetPlacement.transform.position + PICK_POSE_OFFSET).To <FLU>(),
orientation = pickOrientation.To <FLU>()
}
};
ros.Send(rosJointPublishTopicName, request);
}
public void Publish()
{
Debug.Log("Detection made!");
var msg = new RosMessageTypes.Std.Bool(true);
ros.Send(topicName, msg);
}
/// <summary>
///
/// </summary>
void Start()
{
RosMessageTypes.Geometry.Pose[] frame_message = new RosMessageTypes.Geometry.Pose[Arms.Length];
RosMessageTypes.Geometry.Pose[] position_message = new RosMessageTypes.Geometry.Pose[Controllers.Length];
// Get ROS connection static instance
ros = ROSConnection.instance;
parseXYZ(ros, Path + fileName);
int i = 0;
joint_topics = new string[Arms.Length];
frame_topics = new string[Arms.Length];
controller_topics = new string[Controllers.Length];
foreach (GameObject _controller in Controllers)
{
controller_topics[i] = "/unity/" + _controller.name + "/pose/";
position_message[i] = new RosMessageTypes.Geometry.Pose();
i++;
}
i = 0;
foreach (GameObject arm in Arms)
{
frame_message[i] = new RosMessageTypes.Geometry.Pose();
joint_topics[i] = "/unity/" + arm.name + "/joint_state/";
frame_topics[i] = "/unity/" + arm.name + "/base_frame/";
if (arm.name == "ECM")
{
i_ECM = i;
Base_Frames[i] = GameObject.Find("ecm_setup_base_link");
}
else if (arm.name == "PSM1")
{
i_PSM1 = i;
Base_Frames[i] = GameObject.Find(arm.name + "_psm_base_link");
}
else if (arm.name == "PSM2")
{
i_PSM2 = i;
Base_Frames[i] = GameObject.Find(arm.name + "_psm_base_link");
}
i++;
}
for (i = 0; i < Base_Frames.Length; i++)
{
frame_message[i].position = Base_Frames[i].transform.position.To <FLU>();
frame_message[i].orientation = Base_Frames[i].transform.rotation.To <FLU>();
}
i = 0;
for (i = 0; i < frame_message.Length; i++)
{
ros.Send(frame_topics[i], frame_message[i]);
}
}
public void Publish()
{
Vector3 mpos = mobileBase.transform.position;
Vector3 tpos = targetCube.transform.position;
float angle = Mathf.Atan2(tpos.z - mpos.z, tpos.x - mpos.x);
BasePose basePose = new BasePose(tpos.x, tpos.y, tpos.z, angle);
ros.Send(topicName, basePose);
}
// #################################### Setters ######################################
// This is to test when joint positions are broadcast
void broadcastJointPositions()
{
DateTime foo = DateTime.Now;
long unixTime = ((DateTimeOffset)foo).ToUnixTimeSeconds();
JointPositions cmd = new JointPositions(
jointSliderVals[1], // 1 - shoulder
jointSliderVals[2], // 2 - upper arm
jointSliderVals[3], // 3 - forearm
jointSliderVals[4], // 4 - wrist 1
jointSliderVals[5], // 5 - wrist 2
jointSliderVals[6], // 6 - wrist 3
0.0F, // for the gripper
unixTime // unix time so we can ignore commands which are sitting in ROS' pipes but from prev runs
);
// Finally send the message to server_endpoint.py running in ROS
ros.Send("joint_commands", cmd);
}
public void Publish()
{
RosMessageTypes.Geometry.Pose[] frame_message = new RosMessageTypes.Geometry.Pose[Arms.Length];
RosMessageTypes.Geometry.Pose[] position_message = new RosMessageTypes.Geometry.Pose[Controllers.Length];
int i = 0;
foreach (GameObject arm in Arms)
{
frame_message[i] = new RosMessageTypes.Geometry.Pose();
i++;
}
i = 0;
for (i = 0; i < 3; i++)
{
frame_message[i].position = Base_Frames[i].transform.position.To <FLU>();
frame_message[i].orientation = Base_Frames[i].transform.rotation.To <FLU>();
}
i = 0;
foreach (GameObject _controller in Controllers)
{
position_message[i] = new RosMessageTypes.Geometry.Pose();
position_message[i].position = Base_Frames[i].transform.InverseTransformPoint(_controller.transform.position).To <FLU>();
position_message[i].orientation = Relative(Base_Frames[i].transform.rotation, _controller.transform.rotation).To <FLU>();
i++;
}
for (i = 0; i < controller_topics.Length; i++)
{
ros.Send(controller_topics[i], position_message[i]);
}
for (i = 0; i < frame_message.Length; i++)
{
ros.Send(frame_topics[i], frame_message[i]);
}
}
public void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed < publishMessageFrequency)
{
return;
}
ArmPose armPose = arm.GetArmPose();
ros.Send(topicName, armPose);
timeElapsed = 0;
}
private void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed < publishMessageFrequency)
{
return;
}
var pos = arm.worldJoint.transform.InverseTransformPoint(releasePose.transform.position);
var pose = new RosMessageTypes.Geometry.Pose(
new RosMessageTypes.Geometry.Point(pos.x, pos.y, pos.z),
new RosMessageTypes.Geometry.Quaternion());
ros.Send(topicName, pose);
timeElapsed = 0;
}
private void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed < publishMessageFrequency)
{
return;
}
Transform tf = mobileBase.transform;
Vector3 pos = tf.position;
Vector3 pos_d = pos + tf.forward.normalized;
float angle = Mathf.Atan2(pos_d.z - pos.z, pos_d.x - pos.x);
BasePose basePose = new BasePose(pos.x, pos.y, pos.z, angle);
ros.Send(topicName, basePose);
timeElapsed = 0;
}
private void Update()
{
timeElapsed += Time.deltaTime;
if (timeElapsed > publishMessageFrequency)
{
cube.transform.rotation = Random.rotation;
MPosRot cubePos = new MPosRot(
cube.transform.position.x,
cube.transform.position.y,
cube.transform.position.z,
cube.transform.rotation.x,
cube.transform.rotation.y,
cube.transform.rotation.z,
cube.transform.rotation.w
);
// Finally send the message to server_endpoint.py running in ROS
ros.Send(topicName, cubePos);
timeElapsed = 0;
}
}
public void Publish()
{
NiryoMoveitJoints sourceDestinationMessage = new NiryoMoveitJoints();
sourceDestinationMessage.joint_00 = jointArticulationBodies[0].xDrive.target;
sourceDestinationMessage.joint_01 = jointArticulationBodies[1].xDrive.target;
sourceDestinationMessage.joint_02 = jointArticulationBodies[2].xDrive.target;
sourceDestinationMessage.joint_03 = jointArticulationBodies[3].xDrive.target;
sourceDestinationMessage.joint_04 = jointArticulationBodies[4].xDrive.target;
sourceDestinationMessage.joint_05 = jointArticulationBodies[5].xDrive.target;
// Pick Pose
sourceDestinationMessage.pick_pose = new RosMessageTypes.Geometry.Pose
{
position = new Point(
target.transform.position.z,
-target.transform.position.x,
target.transform.position.y
),
orientation = pickOrientation
};
// Place Pose
sourceDestinationMessage.place_pose = new RosMessageTypes.Geometry.Pose
{
position = new Point(
targetPlacement.transform.position.z,
-targetPlacement.transform.position.x,
targetPlacement.transform.position.y
),
orientation = pickOrientation
};
// Finally send the message to server_endpoint.py running in ROS
ros.Send(topicName, sourceDestinationMessage);
}
/// <summary>
/// Publish the joint states from the current robot pose and
/// send to ROS so that it can be used for motion planning
/// </summary>
public void PublishJointStates()
{
int numRobotJoints = jointArticulationBodies.Count;
double[] jointPositions = new double[numRobotJoints];
double[] jointVelocities = new double[numRobotJoints];
double[] jointEfforts = new double[numRobotJoints];
string[] jointNames = new string[numRobotJoints];
jointPositionIndex.Keys.CopyTo(jointNames, 0);
int c = 0;
foreach (ArticulationBody body in jointArticulationBodies)
{
jointPositions[c] = body.jointPosition[0];
jointVelocities[c] = body.jointVelocity[0];
jointEfforts[c] = body.jointFriction;
c += 1;
}
RosMessageTypes.Std.Header header = new RosMessageTypes.Std.Header();
header.stamp = now();
// Pick Pose
RosMessageTypes.Sensor.JointState jointState = new RosMessageTypes.Sensor.JointState
{
header = header,
name = jointNames,
position = jointPositions,
velocity = jointVelocities,
effort = jointEfforts
};
// Finally send the message to server_endpoint.py running in ROS
ros.Send(jointStateTopicName, jointState);
}
void Start()
{
ros.Send(topicName, new RosMessageTypes.Std.Int32(1));
}
public void Publish(bool status, int id)
{
var msg = new TrajectoryStatus(status, id);
ros.Send(topicName, msg);
}