public override void finalize(WalkEnding end, ulong _time)
{
switch (end)
{
case WalkEnding.Identity:
break;
case WalkEnding.TargetParentOfSource:
result_vec = source_to_top_vec;
result_quat = source_to_top_quat;
break;
case WalkEnding.SourceParentOfTarget:
{
emQuaternion inv_target_quat = target_to_top_quat.inverse();
emVector3 inv_target_vec = quatRotate(inv_target_quat, -1 * target_to_top_vec);
result_quat = inv_target_quat;
result_vec = inv_target_vec;
}
break;
case WalkEnding.FullPath:
{
emQuaternion inv_target_quat = target_to_top_quat.inverse();
emVector3 inv_target_vec = quatRotate(inv_target_quat, -1 * target_to_top_vec);
result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
result_quat = inv_target_quat * source_to_top_quat;
}
break;
}
time = _time;
}
public emVector3 quatRotate(emQuaternion rotation, emVector3 v)
{
emQuaternion q = rotation * v;
q *= rotation.inverse();
return(new emVector3(q.x, q.y, q.z));
}
public override void finalize(WalkEnding end, ulong _time)
{
switch (end)
{
case WalkEnding.Identity:
break;
case WalkEnding.TargetParentOfSource:
result_vec = source_to_top_vec;
result_quat = source_to_top_quat;
break;
case WalkEnding.SourceParentOfTarget:
{
emQuaternion inv_target_quat = target_to_top_quat.inverse();
emVector3 inv_target_vec = quatRotate(inv_target_quat, -1 * target_to_top_vec);
result_quat = inv_target_quat;
result_vec = inv_target_vec;
}
break;
case WalkEnding.FullPath:
{
emQuaternion inv_target_quat = target_to_top_quat.inverse();
emVector3 inv_target_vec = quatRotate(inv_target_quat, -1 * target_to_top_vec);
result_vec = quatRotate(inv_target_quat, source_to_top_vec) + inv_target_vec;
result_quat = inv_target_quat * source_to_top_quat;
}
break;
}
time = _time;
}
public TransformStorage(emTransform data, uint frame_id, uint child_frame_id)
{
rotation = data.basis;
translation = data.origin;
stamp = TimeCache.toLong(data.stamp.data);
this.frame_id = frame_id;
this.child_frame_id = child_frame_id;
}
public emTransform(emQuaternion q, emVector3 v, Time t = null, string fid = null, string cfi = null)
{
basis = q;
origin = v;
stamp = t;
frame_id = fid;
child_frame_id = cfi;
}
public void setRotation(emQuaternion q)
{
double d = q.length2();
double s = 2.0/d;
double xs = q.x*s, ys = q.y*s, zs = q.z*s;
double wx = q.w*xs, wy = q.w*ys, wz = q.w*zs;
double xx = q.x*xs, xy = q.x*ys, xz = q.x*zs;
double yy = q.y*ys, yz = q.y*zs, zz = q.z*zs;
setValue(1.0 - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0 - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0 - (xx + yy));
}
public override void accum(bool source)
{
if (source)
{
source_to_top_vec = quatRotate(st.rotation, source_to_top_vec) + st.translation;
source_to_top_quat = st.rotation * source_to_top_quat;
}
else
{
target_to_top_vec = quatRotate(st.rotation, target_to_top_vec) + st.translation;
target_to_top_quat = st.rotation * target_to_top_quat;
}
}
public void setRotation(emQuaternion q)
{
double d = q.length2();
double s = 2.0 / d;
double xs = q.x * s, ys = q.y * s, zs = q.z * s;
double wx = q.w * xs, wy = q.w * ys, wz = q.w * zs;
double xx = q.x * xs, xy = q.x * ys, xz = q.x * zs;
double yy = q.y * ys, yz = q.y * zs, zz = q.z * zs;
setValue(1.0 - (yy + zz), xy - wz, xz + wy,
xy + wz, 1.0 - (xx + zz), yz - wx,
xz - wy, yz + wx, 1.0 - (xx + yy));
}
public override void accum(bool source)
{
if (source)
{
source_to_top_vec = quatRotate(st.rotation, source_to_top_vec) + st.translation;
source_to_top_quat = st.rotation * source_to_top_quat;
}
else
{
target_to_top_vec = quatRotate(st.rotation, target_to_top_vec) + st.translation;
target_to_top_quat = st.rotation * target_to_top_quat;
}
}
void joyPoseCallback(Joy joy)
{
Messages.std_msgs.Time now = ROS.GetTime();
double dt = 0.0;
if ((pose.header.Stamp.data.sec == 0 && pose.header.Stamp.data.nsec == 0))
{
TimeData td = (now - pose.header.Stamp).data;
double sec = td.toSec();
dt = Mathf.Max(0, Mathf.Min(1f, (float)sec));
}
if (getButton(joy, sButtons.go))
{
pose.header.Stamp = ROS.GetTime();
pose.header.Frame_id = worldFrame;
pose.pose.position.x += (Math.Cos(yaw) * getAxis(joy, sAxes.x) - Math.Sin(yaw) * getAxis(joy, sAxes.y)) * dt;
pose.pose.position.y += (Math.Cos(yaw) * getAxis(joy, sAxes.y) + Math.Sin(yaw) * getAxis(joy, sAxes.x)) * dt;
pose.pose.position.z += getAxis(joy, sAxes.z) * dt;
yaw += getAxis(joy, sAxes.yaw) * Math.PI / 180.0 * dt;
tf.net.emQuaternion q = tf.net.emQuaternion.FromRPY(new tf.net.emVector3(0, 0, yaw));
pose.pose.orientation = q.ToMsg();
PoseGoal goal = new PoseGoal();
goal.target_pose = pose;
poseClient.sendGoal(goal);
}
if (getButton(joy, sButtons.interrupt))
{
poseClient.cancelGoalsAtAndBeforeTime(ROS.GetTime());
}
if (getButton(joy, sButtons.stop))
{
landingClient.sendGoalAndWait(new LandingGoal(), new Messages.std_msgs.Duration(new TimeData(10, 0)), new Messages.std_msgs.Duration(new TimeData(10, 0)));
}
}
public emQuaternion slerp(emQuaternion q, double t)
{
double theta = angleShortestPath(q);
if (theta != 0)
{
double d = 1.0 / Math.Sin(theta);
double s0 = Math.Sin(1.0 - t) * theta;
double s1 = Math.Sin(t * theta);
if (dot(q) < 0)
{
return(new emQuaternion(
(w * s0 + -1 * q.w * s1) * d,
(x * s0 + -1 * q.x * s1) * d,
(y * s0 + -1 * q.y * s1) * d,
(z * s0 + -1 * q.z * s1) * d));
}
return(new emQuaternion((w * s0 + q.w * s1) * d,
(x * s0 + q.x * s1) * d,
(y * s0 + q.y * s1) * d,
(z * s0 + q.z * s1) * d));
}
return(new emQuaternion(this));
}
private emQuaternion slerp(emQuaternion q1, emQuaternion q2, double rt)
{
return(q1.slerp(q2, rt));
}
public emTransform(emQuaternion q, emVector3 v, Time t = null, string fid = null, string cfi = null)
{
basis = q;
origin = v;
stamp = t;
frame_id = fid;
child_frame_id = cfi;
}
public emMatrix3x3(emQuaternion q)
{
setRotation(q);
}
public double angleShortestPath(emQuaternion q)
{
return((this - q).abs);
}
public emQuaternion(emQuaternion shallow)
: this(shallow.x, shallow.y, shallow.z, shallow.w)
{
}
public double dot(emQuaternion q)
{
return(x * q.x + y * q.y + z * q.z + w * q.w);
}
public double dot(emQuaternion q)
{
return x*q.x + y*q.y + z*q.z + w*q.w;
}
public emQuaternion(emQuaternion shallow)
: this(shallow.x, shallow.y, shallow.z, shallow.w)
{
}
public TransformStorage()
{
rotation = new emQuaternion();
translation = new emVector3();
}
public double angleShortestPath(emQuaternion q)
{
return (this - q).abs;
}
public emQuaternion slerp(emQuaternion q, double t)
{
double theta = angleShortestPath(q);
if (theta != 0)
{
double d = 1.0/Math.Sin(theta);
double s0 = Math.Sin(1.0 - t)*theta;
double s1 = Math.Sin(t*theta);
if (dot(q) < 0)
{
return new emQuaternion(
(w*s0 + -1*q.w*s1)*d,
(x*s0 + -1*q.x*s1)*d,
(y*s0 + -1*q.y*s1)*d,
(z*s0 + -1*q.z*s1)*d);
}
return new emQuaternion((w*s0 + q.w*s1)*d,
(x*s0 + q.x*s1)*d,
(y*s0 + q.y*s1)*d,
(z*s0 + q.z*s1)*d);
}
return new emQuaternion(this);
}
private emQuaternion slerp(emQuaternion q1, emQuaternion q2, double rt)
{
return q1.slerp(q2, rt);
}
public emMatrix3x3(emQuaternion q)
{
setRotation(q);
}
public emVector3 quatRotate(emQuaternion rotation, emVector3 v)
{
emQuaternion q = rotation * v;
q *= rotation.inverse();
return new emVector3(q.x, q.y, q.z);
}
public void LoadConfig(string filefullname)
{
using (StreamReader reader = File.OpenText(filefullname))
{
YamlStream yaml = new YamlStream();
yaml.Load(reader);
var mapping = (YamlMappingNode)yaml.Documents[0].RootNode;
resolution = Convert.ToDouble(mapping.Children[new YamlScalarNode("resolution")].ToString());
negate = Convert.ToInt32(mapping.Children[new YamlScalarNode("negate")].ToString());
occ_th = Convert.ToDouble(mapping.Children[new YamlScalarNode("occupied_thresh")].ToString());
free_th = Convert.ToDouble(mapping.Children[new YamlScalarNode("free_thresh")].ToString());
try
{
string modeS = "";
modeS = ((YamlScalarNode)mapping.Children[new YamlScalarNode("mode")]).Value;
if (modeS == "trinary")
{
mode = MapMode.TRINARY;
}
else if (modeS == "scale")
{
mode = MapMode.SCALE;
}
else if (modeS == "raw")
{
mode = MapMode.RAW;
}
else
{
return;
}
}
catch (Exception ex)
{
}
origin = new double[3];
origin[0] = Convert.ToDouble(mapping.Children[new YamlScalarNode("origin")][0].ToString());
origin[1] = Convert.ToDouble(mapping.Children[new YamlScalarNode("origin")][1].ToString());
origin[2] = Convert.ToDouble(mapping.Children[new YamlScalarNode("origin")][2].ToString());
mapfname = ((YamlScalarNode)mapping.Children[new YamlScalarNode("image")]).Value;
}
PGM.PGM img = new PGM.PGM(Path.Combine(Path.GetDirectoryName(filefullname), mapfname));
map_resp_.map = new nm.OccupancyGrid();
map_resp_.map.info = new nm.MapMetaData();
map_resp_.map.info.width = (uint)img.Width;
map_resp_.map.info.height = (uint)img.Length;
map_resp_.map.info.resolution = (float)resolution;
map_resp_.map.info.origin = new gm.Pose();
map_resp_.map.info.origin.position = new gm.Point();
map_resp_.map.info.origin.position.x = origin[0];
map_resp_.map.info.origin.position.y = origin[1];
map_resp_.map.info.origin.position.z = 0;
tf.net.emQuaternion q = emQuaternion.FromRPY(new emVector3(0, 0, origin[2]));
map_resp_.map.info.origin.orientation = new gm.Quaternion();
map_resp_.map.info.origin.orientation.x = 0;
map_resp_.map.info.origin.orientation.y = 0;
map_resp_.map.info.origin.orientation.z = 0;
map_resp_.map.info.origin.orientation.w = 1;
map_resp_.map.data = new sbyte[map_resp_.map.info.width * map_resp_.map.info.height];
int rowstride = img.Width;
byte thevalue = 0;
double occ;
for (int j = 0; j < map_resp_.map.info.height; j++)
{
for (int i = 0; i < map_resp_.map.info.width; i++)
{
double color_avg = img.Data[j * rowstride + i];
if (!(negate == 0))
{
color_avg = 255 - color_avg;
}
if (mode == MapMode.RAW)
{
thevalue = (byte)color_avg;
map_resp_.map.data[(map_resp_.map.info.width * (map_resp_.map.info.height - j - 1) + i)] = (sbyte)thevalue;
continue;
}
occ = (255 - color_avg) / 255.0;
if (occ > occ_th)
{
thevalue = +100;
}
else if (occ < free_th)
{
thevalue = 0;
}
else if (mode == MapMode.TRINARY)
{
thevalue = 255;
}
else
{
double ratio = (occ - free_th) / (occ_th - free_th);
thevalue = (byte)(99 * ratio);
}
map_resp_.map.data[map_resp_.map.info.width * (map_resp_.map.info.height - j - 1) + i] = (sbyte)thevalue;
}
}
map_resp_.map.info.map_load_time = ROS.GetTime();
map_resp_.map.header = new m.Header();
map_resp_.map.header.frame_id = fram_id;
map_resp_.map.header.stamp = ROS.GetTime();
mete_data_message_ = map_resp_.map.info;
}