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; } }