public void RequestSetParams(string rosDeviceName)
{
GameObject target = GameObject.Find(rosDeviceName);
Vector3 RosPosition = CoordinateConvert.UnityToRos(target.transform.position);
string pos_x_key = $"/lidars/{rosDeviceName}/transform/translation/x";
string pos_y_key = $"/lidars/{rosDeviceName}/transform/translation/y";
string pos_z_key = $"/lidars/{rosDeviceName}/transform/translation/z";
string pos_x_value = RosPosition.x.ToString();
string pos_y_value = RosPosition.y.ToString();
string pos_z_value = RosPosition.z.ToString();
// ��]��quaternion�ŕ\�������
Quaternion RosQuaternion = CoordinateConvert.UnityToRos(target.transform.rotation);
string rotation_x_key = $"/lidars/{rosDeviceName}/transform/rotation/x";
string rotation_y_key = $"/lidars/{rosDeviceName}/transform/rotation/y";
string rotation_z_key = $"/lidars/{rosDeviceName}/transform/rotation/z";
string rotation_w_key = $"/lidars/{rosDeviceName}/transform/rotation/w";
string rotation_x_value = RosQuaternion.x.ToString();
string rotation_y_value = RosQuaternion.y.ToString();
string rotation_z_value = RosQuaternion.z.ToString();
string rotation_w_value = RosQuaternion.w.ToString();
SetSingleValueToRosparam(pos_x_key, pos_x_value);
SetSingleValueToRosparam(pos_y_key, pos_y_value);
SetSingleValueToRosparam(pos_z_key, pos_z_value);
SetSingleValueToRosparam(rotation_x_key, rotation_x_value);
SetSingleValueToRosparam(rotation_y_key, rotation_y_value);
SetSingleValueToRosparam(rotation_z_key, rotation_z_value);
SetSingleValueToRosparam(rotation_w_key, rotation_w_value);
}
private void SetRegionsToRosparam()
{
Debug.Log("SetRegionsToRosparam");
regionGameObjects = GameObject.FindGameObjectsWithTag("RegionForeground");
Region[] regions = new Region[regionGameObjects.Length];
bool flag_changed = false;
if (regionGameObjects.Length != regionsFromRosparam.Length)
{
flag_changed = true;
}
for (int i = 0; i < regionGameObjects.Length; i++)
{
Vector3 p = CoordinateConvert.UnityToRos(regionGameObjects[i].transform.position);
Vector3 _s = CoordinateConvert.UnityToRos(regionGameObjects[i].transform.localScale);
Vector3 s = new Vector3(Mathf.Abs(_s.x), Mathf.Abs(_s.y), Mathf.Abs(_s.z));
Quaternion q = CoordinateConvert.UnityToRos(regionGameObjects[i].transform.rotation);
Vector3 minPt = new Vector3(p.x - s.x / 2, p.y - s.y / 2, p.z - s.z / 2);
Vector3 maxPt = new Vector3(p.x + s.x / 2, p.y + s.y / 2, p.z + s.z / 2);
regions[i] = new Region();
regions[i].minPt = new Point(minPt.x, minPt.y, minPt.z);
regions[i].maxPt = new Point(maxPt.x, maxPt.y, maxPt.z);
regions[i].rotation = new Point(q.eulerAngles.x, q.eulerAngles.y, q.eulerAngles.z);
if (!flag_changed && JsonUtility.ToJson(regions[i]) != JsonUtility.ToJson(regionsFromRosparam[i]))
{
flag_changed = true;
}
}
if (!flag_changed)
{
return;
}
if (rosConnection != null && rosConnection.RosSocket != null)
{
for (int i = 0; i < regions.Length; i++)
{
string key_min_x = $"/regions/foregrounds/{i}/minPt/x";
string key_min_y = $"/regions/foregrounds/{i}/minPt/y";
string key_min_z = $"/regions/foregrounds/{i}/minPt/z";
string key_max_x = $"/regions/foregrounds/{i}/maxPt/x";
string key_max_y = $"/regions/foregrounds/{i}/maxPt/y";
string key_max_z = $"/regions/foregrounds/{i}/maxPt/z";
string key_rotation_x = $"/regions/foregrounds/{i}/rotation/x";
string key_rotation_y = $"/regions/foregrounds/{i}/rotation/y";
string key_rotation_z = $"/regions/foregrounds/{i}/rotation/z";
string value_min_x = regions[i].minPt.x.ToString("F4");
string value_min_y = regions[i].minPt.y.ToString("F4");
string value_min_z = regions[i].minPt.z.ToString("F4");
string value_max_y = regions[i].maxPt.y.ToString("F4");
string value_max_x = regions[i].maxPt.x.ToString("F4");
string value_max_z = regions[i].maxPt.z.ToString("F4");
string value_rot_y = regions[i].rotation.x.ToString("F4");
string value_rot_x = regions[i].rotation.y.ToString("F4");
string value_rot_z = regions[i].rotation.z.ToString("F4");
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_min_x, value_min_x));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_min_y, value_min_y));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_min_z, value_min_z));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_max_x, value_max_x));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_max_y, value_max_y));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_max_z, value_max_z));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_rotation_x, value_rot_x));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_rotation_y, value_rot_y));
rosConnection.RosSocket.CallService <rosapi.SetParamRequest, rosapi.SetParamResponse>("/rosapi/set_param", setValueResultHandler, new rosapi.SetParamRequest(key_rotation_z, value_rot_z));
Debug.Log("Set: " + JsonUtility.ToJson(regions[i]));
}
// rosparam側に多い分は削除する
for (int i = regions.Length; i < regionsFromRosparam.Length; i++)
{
string delete_key = $"/regions/foregrounds/{i}";
rosConnection.RosSocket.CallService <rosapi.DeleteParamRequest, rosapi.DeleteParamResponse>("/rosapi/delete_param", deleteParamResultHandler, new rosapi.DeleteParamRequest(delete_key));
Debug.Log("delete key");
}
regionsFromRosparam = regions;
}
}