//called when restart
void OnEnable()
{
if (objectToPick != null && programItem != null)
{
objectToPick.SetActive(true);
PoseMsg gripper_pose = programItem.GetPose()[0].GetPose();
objectToPick.transform.localPosition = new Vector3(gripper_pose.GetPosition().GetX(), -gripper_pose.GetPosition().GetY(), gripper_pose.GetPosition().GetZ());
if (left_feeder)
{
//rotate the object to make right move
objectToPick.transform.localEulerAngles = new Vector3(-90f, 90f, 0f);
//position the object to fit approximately on the feeder
objectToPick.transform.localPosition += new Vector3(-0.30f, 0f, 0f);
}
else
{
//rotate the object to make right move
objectToPick.transform.localEulerAngles = new Vector3(-90f, -90f, 0f);
//position the object to fit approximately on the feeder
objectToPick.transform.localPosition += new Vector3(0.30f, 0f, 0f);
}
//InitRobotGripper();
PlaceRobotGripperToInit();
}
}
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);
}
}
public MapMetaDataMsg(TimeMsg map_load_time, float resolution, uint width, uint height, PoseMsg origin)
{
_map_load_time = map_load_time;
_resolution = resolution;
_width = width;
_height = height;
_origin = origin;
}
public MapMetaDataInfoMsg(JSONNode node)
{
_time = node["map_load_time"];
_resolution = node["resolution"].AsFloat;
_width = node["width"].AsInt;
_height = node["height"].AsInt;
_origin = new PoseMsg(node["origin"]);
}
private void InitPlacePose()
{
placePose = programItem.GetPose()[0].GetPose();
placePosition = new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), placePose.GetPosition().GetZ());
//placePosition = new Vector3(placePose.GetPosition().GetX(), -placePose.GetPosition().GetY(), 0f);
placeQuaternion = new Quaternion(-placePose.GetOrientation().GetX(), placePose.GetOrientation().GetY(), -placePose.GetOrientation().GetZ(), placePose.GetOrientation().GetW());
upPosition = objectToPlace.transform.localPosition + new Vector3(0, 0, 0.1f);
}
public SubmapEntryMsg(JSONNode node)
{
this.trajectory_id = node["trajectory_id"].AsInt;
this.submap_index = node["submap_index"].AsInt;
this.submap_version = node["submap_version"].AsInt;
this.pose = new PoseMsg(node["pose"]);
this.is_frozen = node["is_frozen"].AsBool;
}
public DetectionArrayMsg(JSONNode msg) {
header = new HeaderMsg(msg["header"]);
origin = new PoseMsg(msg["origin"]);
detections = new DetectionMsg[msg["detections"].Count];
for (int i = 0; i < detections.Length; i++) {
detections[i] = new DetectionMsg(msg["detections"][i]);
}
}
public ObjInstanceMsg(string object_id, string object_type, PoseMsg pose, List <KeyValueMsg> flags, bool on_table)
{
_object_id = object_id;
_object_type = object_type;
_pose = pose;
_flags = flags;
_on_table = on_table;
}
public SubmapEntryMsg(int trajectory_id, int submap_index, int submap_version, PoseMsg pose, bool is_frozen)
{
this.trajectory_id = trajectory_id;
this.submap_index = submap_index;
this.submap_version = submap_version;
this.pose = pose;
this.is_frozen = is_frozen;
}
public MapMetaDataMsg(JSONNode msg)
{
_map_load_time = new TimeMsg(msg["map_load_time"]);
_resolution = float.Parse(msg["resolution"], System.Globalization.CultureInfo.InvariantCulture);
_width = uint.Parse(msg["width"], System.Globalization.CultureInfo.InvariantCulture);
_height = uint.Parse(msg["height"], System.Globalization.CultureInfo.InvariantCulture);
_origin = new PoseMsg(msg["origin"]);
}
public SubmapTextureMsg(byte[] cells, int width, int height, float resolution, PoseMsg slice_pose)
{
this.cells = cells;
this.width = width;
this.height = height;
this.resolution = resolution;
this.slice_pose = slice_pose;
}
public PoseWithCovarianceMsg(JSONNode msg)
{
_pose = new PoseMsg(msg["pose"]);
// Treat covariance
for (int i = 0; i < msg["covariance"].Count; i++)
{
_covariance[i] = double.Parse(msg["covariance"][i]);
}
}
void AddMessage(PoseMsg message)
{
m_LastMessage = message;
if (m_IsDrawingEnabled)
{
Redraw();
}
}
public override void Deserialize(JSONNode msg)
{
_pose = new PoseMsg(msg["pose"]);
// Treat covariance
for (int i = 0; i < msg["covariance"].Count; i++)
{
_covariance[i] = double.Parse(msg["covariance"][i]);
}
}
public PoseWithCovarianceMsg(JSONNode msg)
{
_pose = new PoseMsg(msg["pose"]);
_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 PoseMsg message, string name = "")
{
if (name.Length > 0)
{
GUILayout.Label(name);
}
message.position.GUI("Position");
message.orientation.GUI("Orientation");
}
public SemanticObjectMsg(JSONNode msg)
{
_id = msg["id"];
_idRoom = msg["idRoom"];
_type = msg["type"];
_score = double.Parse(msg["score"], System.Globalization.CultureInfo.InvariantCulture);
_pointCloud = new PointCloudMsg(msg["pointCloud"]);
_pose = new PoseMsg(msg["pose"]);
_scale = new Vector3Msg(msg["scale"]);
}
public void sendPingerPos()
{
Vector3 posTmp = pinger.transform.position;
Quaternion rotTmp = pinger.transform.rotation;
pingerPoint = new PointMsg(posTmp.x, posTmp.y, posTmp.z);
pingerQuaternion = new QuaternionMsg(rotTmp.x, rotTmp.y, rotTmp.z, rotTmp.w);
pingerMsg = new PoseMsg(pingerPoint, pingerQuaternion);
obj.GetComponent <ROSInitializerSAUVC>().rosSAUVC.Publish(PingerPublisher.GetMessageTopic(), pingerMsg);
}
public static void GUI(this PoseArrayMsg message)
{
GUI(message.header);
for (int Idx = 0; Idx < message.poses.Length; ++Idx)
{
PoseMsg pose = message.poses[Idx];
pose.position.GUI($"[{Idx}] Position");
pose.orientation.GUI("Orientation");
}
}
public SemanticObjectMsg(string id, ObjectHypothesisMsg[] scores, PoseMsg pose, int ndetections, String roomId, String roomType, Vector3Msg size)
{
_id = id;
_scores = scores;
_pose = pose;
_detections = ndetections;
_roomId = roomId;
_roomType = roomType;
_size = size;
}
public UserRGBDMsg(string name, float u, float v, float w, float h, PoseMsg pose_3d, float certainty)
{
_name = name;
_u = u;
_v = v;
_w = w;
_h = h;
_pose_3D = pose_3d;
_certainty = certainty;
}
public SemanticObjectMsg(String id, String idRoom, String type, float score, PointCloudMsg pointCloud, PoseMsg position, Vector3Msg scale)
{
_id = id;
_idRoom = idRoom;
_type = type;
_score = score;
_pointCloud = pointCloud;
_pose = position;
_scale = scale;
}
public ObjInstanceMsg(JSONNode msg)
{
_object_id = msg["object_id"];
_object_type = msg["object_type"];
_pose = new PoseMsg(msg["pose"]);
foreach (JSONNode item in msg["flags"].AsArray)
{
_flags.Add(new KeyValueMsg(item));
}
_on_table = bool.Parse(msg["on_table"]);
}
public void SendPos(int id)
{
if (id != 0 && id <= props.Length)
{
Vector3 posTmp = props[id - 1].transform.position;
Quaternion rotTmp = props[id - 1].transform.rotation;
pt = new PointMsg(posTmp.x, posTmp.y, posTmp.z);
qt = new QuaternionMsg(rotTmp.x, rotTmp.y, rotTmp.z, rotTmp.w);
posMsg = new PoseMsg(pt, qt);
obj.GetComponent <ROSInitializerSAUVC>().rosSAUVC.Publish(PosPublisher.GetMessageTopic(), posMsg);
}
}
//inits poses of robot arm for drilling holes
private void InitDrillingPoses()
{
List <PoseStampedMsg> poses = programItem.GetPose();
PoseMsg position1 = poses[0].GetPose();
PoseMsg position2 = poses[1].GetPose();
//robots gripper local position starts at the grippers joint (not the tip).. so place the joint position as initial position for tip before drilling
pose1_init = new Vector3(position1.GetPosition().GetX(), -position1.GetPosition().GetY(), position1.GetPosition().GetZ());
pose2_init = new Vector3(position2.GetPosition().GetX(), -position2.GetPosition().GetY(), position2.GetPosition().GetZ());
//drilling_hole_1_init = true;
drilling_State = drilling_state.INIT_DRILLING_HOLE_1;
}
public PoseWithCovarianceMsg(PoseMsg pose, double[] covariance)
{
_pose = pose;
if (covariance.Length == 36)
{
_covariance = covariance;
}
else
{
_covariance = new double[36];
}
}
public SemanticObjectMsg(JSONNode msg)
{
_id = msg["id"];
_scores = new ObjectHypothesisMsg[msg["scores"].Count];
for (int i = 0; i < _scores.Length; i++)
{
_scores[i] = new ObjectHypothesisMsg(msg["scores"][i]);
}
_pose = new PoseMsg(msg["pose"]);
_detections = int.Parse(msg["detections"]);
_roomId = msg["roomId"];
_roomType = msg["roomType"];
_size = new Vector3Msg(msg["size"]);
}
public new static void CallBack(ROSBridgeMsg msg)
{
GameObject robot = GameObject.Find("Dalek");
if (robot == null)
{
Debug.Log("Can't find the robot???");
}
else
{
PoseMsg pose = (PoseMsg)msg;
robot.transform.position = new Vector3(pose.GetX(), 0.2f, pose.GetY());
robot.transform.rotation = Quaternion.AngleAxis(-pose.GetTheta() * 180.0f / 3.1415f, Vector3.up);
}
}
private static bool PoseSet(PoseMsg pose)
{
if (pose.GetPosition().GetX() == 0f &&
pose.GetPosition().GetY() == 0f &&
pose.GetPosition().GetZ() == 0f &&
pose.GetOrientation().GetX() == 0f &&
pose.GetOrientation().GetY() == 0f &&
pose.GetOrientation().GetZ() == 0f &&
pose.GetOrientation().GetW() == 0f)
{
return(false);
}
return(true);
}
public UserRGBDMsg(JSONNode msg)
{
_name = msg["name"];
_u = float.Parse(msg["u"], System.Globalization.CultureInfo.InvariantCulture);
_v = float.Parse(msg["v"], System.Globalization.CultureInfo.InvariantCulture);
_w = float.Parse(msg["w"], System.Globalization.CultureInfo.InvariantCulture);
_h = float.Parse(msg["h"], System.Globalization.CultureInfo.InvariantCulture);
_pose_3D = new PoseMsg(msg["pose_3D"]);
_certainty = float.Parse(msg["certainty"], System.Globalization.CultureInfo.InvariantCulture);
_body_part_3d = new BodyPart3DMsg[msg["body_part_3d"].Count];
for (int i = 0; i < _body_part_3d.Length; i++)
{
_body_part_3d[i] = new BodyPart3DMsg(msg["body_part_3d"][i]);
}
}
