public override void deserialize(MemoryStream stream, halodi_msgs.msg.TaskSpaceCommand data)
{
using (BinaryReader reader = new BinaryReader(stream))
{
Halodi.CDR.CDRDeserializer cdr = new Halodi.CDR.CDRDeserializer(reader);
read(data, cdr);
}
}
public override void serialize(halodi_msgs.msg.TaskSpaceCommand data, MemoryStream stream)
{
using (BinaryWriter writer = new BinaryWriter(stream))
{
Halodi.CDR.CDRSerializer cdr = new Halodi.CDR.CDRSerializer(writer);
write(data, cdr);
}
}
public static int getCdrSerializedSize(halodi_msgs.msg.TaskSpaceCommand data, int current_alignment)
{
int initial_alignment = current_alignment;
current_alignment += halodi_msgs.msg.ReferenceFrameNamePubSubType.getCdrSerializedSize(data.body_frame, current_alignment);
current_alignment += halodi_msgs.msg.ReferenceFrameNamePubSubType.getCdrSerializedSize(data.expressed_in_frame, current_alignment);
current_alignment += 1 + Halodi.CDR.CDRCommon.alignment(current_alignment, 1);
current_alignment += geometry_msgs.msg.PosePubSubType.getCdrSerializedSize(data.pose, current_alignment);
current_alignment += geometry_msgs.msg.Vector3PubSubType.getCdrSerializedSize(data.angular_velocity, current_alignment);
current_alignment += geometry_msgs.msg.Vector3PubSubType.getCdrSerializedSize(data.linear_velocity, current_alignment);
current_alignment += geometry_msgs.msg.Vector3PubSubType.getCdrSerializedSize(data.angular_acceleration, current_alignment);
current_alignment += geometry_msgs.msg.Vector3PubSubType.getCdrSerializedSize(data.linear_acceleration, current_alignment);
current_alignment += 4 + Halodi.CDR.CDRCommon.alignment(current_alignment, 4);
for (int i0 = 0; i0 < data.passivity_input.Count; ++i0)
{
current_alignment += halodi_msgs.msg.PassivityDataPubSubType.getCdrSerializedSize(data.passivity_input[i0], current_alignment);
}
current_alignment += 4 + Halodi.CDR.CDRCommon.alignment(current_alignment, 4);
for (int i0 = 0; i0 < data.orientation_feedback_parameters.Count; ++i0)
{
current_alignment += halodi_msgs.msg.FeedbackParameters3DPubSubType.getCdrSerializedSize(data.orientation_feedback_parameters[i0], current_alignment);
}
current_alignment += 4 + Halodi.CDR.CDRCommon.alignment(current_alignment, 4);
for (int i0 = 0; i0 < data.position_feedback_parameters.Count; ++i0)
{
current_alignment += halodi_msgs.msg.FeedbackParameters3DPubSubType.getCdrSerializedSize(data.position_feedback_parameters[i0], current_alignment);
}
current_alignment += 4 + Halodi.CDR.CDRCommon.alignment(current_alignment, 4);
for (int i0 = 0; i0 < data.nullspace_command.Count; ++i0)
{
current_alignment += halodi_msgs.msg.JointNullSpaceConfigurationPubSubType.getCdrSerializedSize(data.nullspace_command[i0], current_alignment);
}
return(current_alignment - initial_alignment);
}
public void Set(WholeBodyControllerCommand other)
{
sequence_id = other.sequence_id;
halodi_msgs.msg.BalanceModePubSubType.Copy(other.balance_mode, balance_mode);
if (other.task_space_commands == null)
{
task_space_commands = null;
}
else
{
task_space_commands = new System.Collections.Generic.List <halodi_msgs.msg.TaskSpaceCommand>(other.task_space_commands.Count);
for (int i1 = 0; i1 < other.task_space_commands.Count; i1++)
{
if (other.task_space_commands[i1] == null)
{
task_space_commands.Add(null);
}
else
{
halodi_msgs.msg.TaskSpaceCommand newElement = halodi_msgs.msg.TaskSpaceCommandPubSubType.Create();
halodi_msgs.msg.TaskSpaceCommandPubSubType.Copy(other.task_space_commands[i1], newElement);
task_space_commands.Add(newElement);
}
}
}
if (other.joint_space_commands == null)
{
joint_space_commands = null;
}
else
{
joint_space_commands = new System.Collections.Generic.List <halodi_msgs.msg.JointSpaceCommand>(other.joint_space_commands.Count);
for (int i2 = 0; i2 < other.joint_space_commands.Count; i2++)
{
if (other.joint_space_commands[i2] == null)
{
joint_space_commands.Add(null);
}
else
{
halodi_msgs.msg.JointSpaceCommand newElement = halodi_msgs.msg.JointSpaceCommandPubSubType.Create();
halodi_msgs.msg.JointSpaceCommandPubSubType.Copy(other.joint_space_commands[i2], newElement);
joint_space_commands.Add(newElement);
}
}
}
if (other.contact_point_commands == null)
{
contact_point_commands = null;
}
else
{
contact_point_commands = new System.Collections.Generic.List <halodi_msgs.msg.ContactPointCommand>(other.contact_point_commands.Count);
for (int i3 = 0; i3 < other.contact_point_commands.Count; i3++)
{
if (other.contact_point_commands[i3] == null)
{
contact_point_commands.Add(null);
}
else
{
halodi_msgs.msg.ContactPointCommand newElement = halodi_msgs.msg.ContactPointCommandPubSubType.Create();
halodi_msgs.msg.ContactPointCommandPubSubType.Copy(other.contact_point_commands[i3], newElement);
contact_point_commands.Add(newElement);
}
}
}
if (other.driving_command == null)
{
driving_command = null;
}
else
{
driving_command = new System.Collections.Generic.List <halodi_msgs.msg.DrivingCommand>(other.driving_command.Count);
for (int i4 = 0; i4 < other.driving_command.Count; i4++)
{
if (other.driving_command[i4] == null)
{
driving_command.Add(null);
}
else
{
halodi_msgs.msg.DrivingCommand newElement = halodi_msgs.msg.DrivingCommandPubSubType.Create();
halodi_msgs.msg.DrivingCommandPubSubType.Copy(other.driving_command[i4], newElement);
driving_command.Add(newElement);
}
}
}
if (other.icp_command == null)
{
icp_command = null;
}
else
{
icp_command = new System.Collections.Generic.List <geometry_msgs.msg.Vector3>(other.icp_command.Count);
for (int i5 = 0; i5 < other.icp_command.Count; i5++)
{
if (other.icp_command[i5] == null)
{
icp_command.Add(null);
}
else
{
geometry_msgs.msg.Vector3 newElement = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.Copy(other.icp_command[i5], newElement);
icp_command.Add(newElement);
}
}
}
}
public static void read(halodi_msgs.msg.WholeBodyTrajectoryPoint data, Halodi.CDR.CDRDeserializer cdr)
{
data.time_from_start = builtin_interfaces.msg.DurationPubSubType.Create();
builtin_interfaces.msg.DurationPubSubType.read(data.time_from_start, cdr);
int task_space_commands_length = cdr.read_type_2();
data.task_space_commands = new System.Collections.Generic.List <halodi_msgs.msg.TaskSpaceCommand>(task_space_commands_length);
for (int i = 0; i < task_space_commands_length; i++)
{
halodi_msgs.msg.TaskSpaceCommand new_task_space_commands = halodi_msgs.msg.TaskSpaceCommandPubSubType.Create();
halodi_msgs.msg.TaskSpaceCommandPubSubType.read(new_task_space_commands, cdr);
data.task_space_commands.Add(new_task_space_commands);
}
int joint_space_commands_length = cdr.read_type_2();
data.joint_space_commands = new System.Collections.Generic.List <halodi_msgs.msg.JointSpaceCommand>(joint_space_commands_length);
for (int i = 0; i < joint_space_commands_length; i++)
{
halodi_msgs.msg.JointSpaceCommand new_joint_space_commands = halodi_msgs.msg.JointSpaceCommandPubSubType.Create();
halodi_msgs.msg.JointSpaceCommandPubSubType.read(new_joint_space_commands, cdr);
data.joint_space_commands.Add(new_joint_space_commands);
}
int contact_point_commands_length = cdr.read_type_2();
data.contact_point_commands = new System.Collections.Generic.List <halodi_msgs.msg.ContactPointCommand>(contact_point_commands_length);
for (int i = 0; i < contact_point_commands_length; i++)
{
halodi_msgs.msg.ContactPointCommand new_contact_point_commands = halodi_msgs.msg.ContactPointCommandPubSubType.Create();
halodi_msgs.msg.ContactPointCommandPubSubType.read(new_contact_point_commands, cdr);
data.contact_point_commands.Add(new_contact_point_commands);
}
int icp_offset_command_length = cdr.read_type_2();
data.icp_offset_command = new System.Collections.Generic.List <geometry_msgs.msg.Vector3>(icp_offset_command_length);
for (int i = 0; i < icp_offset_command_length; i++)
{
geometry_msgs.msg.Vector3 new_icp_offset_command = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.read(new_icp_offset_command, cdr);
data.icp_offset_command.Add(new_icp_offset_command);
}
int left_hand_command_length = cdr.read_type_2();
data.left_hand_command = new System.Collections.Generic.List <halodi_msgs.msg.HandCommand>(left_hand_command_length);
for (int i = 0; i < left_hand_command_length; i++)
{
halodi_msgs.msg.HandCommand new_left_hand_command = halodi_msgs.msg.HandCommandPubSubType.Create();
halodi_msgs.msg.HandCommandPubSubType.read(new_left_hand_command, cdr);
data.left_hand_command.Add(new_left_hand_command);
}
int right_hand_command_length = cdr.read_type_2();
data.right_hand_command = new System.Collections.Generic.List <halodi_msgs.msg.HandCommand>(right_hand_command_length);
for (int i = 0; i < right_hand_command_length; i++)
{
halodi_msgs.msg.HandCommand new_right_hand_command = halodi_msgs.msg.HandCommandPubSubType.Create();
halodi_msgs.msg.HandCommandPubSubType.read(new_right_hand_command, cdr);
data.right_hand_command.Add(new_right_hand_command);
}
}
public void Set(WholeBodyTrajectoryPoint other)
{
builtin_interfaces.msg.DurationPubSubType.Copy(other.time_from_start, time_from_start);
if (other.task_space_commands == null)
{
task_space_commands = null;
}
else
{
task_space_commands = new System.Collections.Generic.List <halodi_msgs.msg.TaskSpaceCommand>(other.task_space_commands.Count);
for (int i1 = 0; i1 < other.task_space_commands.Count; i1++)
{
if (other.task_space_commands[i1] == null)
{
task_space_commands.Add(null);
}
else
{
halodi_msgs.msg.TaskSpaceCommand newElement = halodi_msgs.msg.TaskSpaceCommandPubSubType.Create();
halodi_msgs.msg.TaskSpaceCommandPubSubType.Copy(other.task_space_commands[i1], newElement);
task_space_commands.Add(newElement);
}
}
}
if (other.joint_space_commands == null)
{
joint_space_commands = null;
}
else
{
joint_space_commands = new System.Collections.Generic.List <halodi_msgs.msg.JointSpaceCommand>(other.joint_space_commands.Count);
for (int i2 = 0; i2 < other.joint_space_commands.Count; i2++)
{
if (other.joint_space_commands[i2] == null)
{
joint_space_commands.Add(null);
}
else
{
halodi_msgs.msg.JointSpaceCommand newElement = halodi_msgs.msg.JointSpaceCommandPubSubType.Create();
halodi_msgs.msg.JointSpaceCommandPubSubType.Copy(other.joint_space_commands[i2], newElement);
joint_space_commands.Add(newElement);
}
}
}
if (other.contact_point_commands == null)
{
contact_point_commands = null;
}
else
{
contact_point_commands = new System.Collections.Generic.List <halodi_msgs.msg.ContactPointCommand>(other.contact_point_commands.Count);
for (int i3 = 0; i3 < other.contact_point_commands.Count; i3++)
{
if (other.contact_point_commands[i3] == null)
{
contact_point_commands.Add(null);
}
else
{
halodi_msgs.msg.ContactPointCommand newElement = halodi_msgs.msg.ContactPointCommandPubSubType.Create();
halodi_msgs.msg.ContactPointCommandPubSubType.Copy(other.contact_point_commands[i3], newElement);
contact_point_commands.Add(newElement);
}
}
}
if (other.icp_offset_command == null)
{
icp_offset_command = null;
}
else
{
icp_offset_command = new System.Collections.Generic.List <geometry_msgs.msg.Vector3>(other.icp_offset_command.Count);
for (int i4 = 0; i4 < other.icp_offset_command.Count; i4++)
{
if (other.icp_offset_command[i4] == null)
{
icp_offset_command.Add(null);
}
else
{
geometry_msgs.msg.Vector3 newElement = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.Copy(other.icp_offset_command[i4], newElement);
icp_offset_command.Add(newElement);
}
}
}
if (other.left_hand_command == null)
{
left_hand_command = null;
}
else
{
left_hand_command = new System.Collections.Generic.List <halodi_msgs.msg.HandCommand>(other.left_hand_command.Count);
for (int i5 = 0; i5 < other.left_hand_command.Count; i5++)
{
if (other.left_hand_command[i5] == null)
{
left_hand_command.Add(null);
}
else
{
halodi_msgs.msg.HandCommand newElement = halodi_msgs.msg.HandCommandPubSubType.Create();
halodi_msgs.msg.HandCommandPubSubType.Copy(other.left_hand_command[i5], newElement);
left_hand_command.Add(newElement);
}
}
}
if (other.right_hand_command == null)
{
right_hand_command = null;
}
else
{
right_hand_command = new System.Collections.Generic.List <halodi_msgs.msg.HandCommand>(other.right_hand_command.Count);
for (int i6 = 0; i6 < other.right_hand_command.Count; i6++)
{
if (other.right_hand_command[i6] == null)
{
right_hand_command.Add(null);
}
else
{
halodi_msgs.msg.HandCommand newElement = halodi_msgs.msg.HandCommandPubSubType.Create();
halodi_msgs.msg.HandCommandPubSubType.Copy(other.right_hand_command[i6], newElement);
right_hand_command.Add(newElement);
}
}
}
}
public static void read(halodi_msgs.msg.WholeBodyControllerCommand data, Halodi.CDR.CDRDeserializer cdr)
{
data.sequence_id = cdr.read_type_2();
data.balance_mode = halodi_msgs.msg.BalanceModePubSubType.Create();
halodi_msgs.msg.BalanceModePubSubType.read(data.balance_mode, cdr);
int task_space_commands_length = cdr.read_type_2();
data.task_space_commands = new System.Collections.Generic.List <halodi_msgs.msg.TaskSpaceCommand>(task_space_commands_length);
for (int i = 0; i < task_space_commands_length; i++)
{
halodi_msgs.msg.TaskSpaceCommand new_task_space_commands = halodi_msgs.msg.TaskSpaceCommandPubSubType.Create();
halodi_msgs.msg.TaskSpaceCommandPubSubType.read(new_task_space_commands, cdr);
data.task_space_commands.Add(new_task_space_commands);
}
int joint_space_commands_length = cdr.read_type_2();
data.joint_space_commands = new System.Collections.Generic.List <halodi_msgs.msg.JointSpaceCommand>(joint_space_commands_length);
for (int i = 0; i < joint_space_commands_length; i++)
{
halodi_msgs.msg.JointSpaceCommand new_joint_space_commands = halodi_msgs.msg.JointSpaceCommandPubSubType.Create();
halodi_msgs.msg.JointSpaceCommandPubSubType.read(new_joint_space_commands, cdr);
data.joint_space_commands.Add(new_joint_space_commands);
}
int contact_point_commands_length = cdr.read_type_2();
data.contact_point_commands = new System.Collections.Generic.List <halodi_msgs.msg.ContactPointCommand>(contact_point_commands_length);
for (int i = 0; i < contact_point_commands_length; i++)
{
halodi_msgs.msg.ContactPointCommand new_contact_point_commands = halodi_msgs.msg.ContactPointCommandPubSubType.Create();
halodi_msgs.msg.ContactPointCommandPubSubType.read(new_contact_point_commands, cdr);
data.contact_point_commands.Add(new_contact_point_commands);
}
int driving_command_length = cdr.read_type_2();
data.driving_command = new System.Collections.Generic.List <halodi_msgs.msg.DrivingCommand>(driving_command_length);
for (int i = 0; i < driving_command_length; i++)
{
halodi_msgs.msg.DrivingCommand new_driving_command = halodi_msgs.msg.DrivingCommandPubSubType.Create();
halodi_msgs.msg.DrivingCommandPubSubType.read(new_driving_command, cdr);
data.driving_command.Add(new_driving_command);
}
int icp_command_length = cdr.read_type_2();
data.icp_command = new System.Collections.Generic.List <geometry_msgs.msg.Vector3>(icp_command_length);
for (int i = 0; i < icp_command_length; i++)
{
geometry_msgs.msg.Vector3 new_icp_command = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.read(new_icp_command, cdr);
data.icp_command.Add(new_icp_command);
}
}
public static void write(halodi_msgs.msg.TaskSpaceCommand data, Halodi.CDR.CDRSerializer cdr)
{
halodi_msgs.msg.ReferenceFrameNamePubSubType.write(data.body_frame, cdr);
halodi_msgs.msg.ReferenceFrameNamePubSubType.write(data.expressed_in_frame, cdr);
cdr.write_type_7(data.express_in_z_up);
geometry_msgs.msg.PosePubSubType.write(data.pose, cdr);
geometry_msgs.msg.Vector3PubSubType.write(data.angular_velocity, cdr);
geometry_msgs.msg.Vector3PubSubType.write(data.linear_velocity, cdr);
geometry_msgs.msg.Vector3PubSubType.write(data.angular_acceleration, cdr);
geometry_msgs.msg.Vector3PubSubType.write(data.linear_acceleration, cdr);
if (data.passivity_input == null)
{
cdr.write_type_2(0);
}
else
{
int passivity_input_length = data.passivity_input.Count;
cdr.write_type_2(passivity_input_length);
for (int i0 = 0; i0 < passivity_input_length; i0++)
{
halodi_msgs.msg.PassivityDataPubSubType.write(data.passivity_input[i0], cdr);
}
}
if (data.orientation_feedback_parameters == null)
{
cdr.write_type_2(0);
}
else
{
int orientation_feedback_parameters_length = data.orientation_feedback_parameters.Count;
cdr.write_type_2(orientation_feedback_parameters_length);
for (int i0 = 0; i0 < orientation_feedback_parameters_length; i0++)
{
halodi_msgs.msg.FeedbackParameters3DPubSubType.write(data.orientation_feedback_parameters[i0], cdr);
}
}
if (data.position_feedback_parameters == null)
{
cdr.write_type_2(0);
}
else
{
int position_feedback_parameters_length = data.position_feedback_parameters.Count;
cdr.write_type_2(position_feedback_parameters_length);
for (int i0 = 0; i0 < position_feedback_parameters_length; i0++)
{
halodi_msgs.msg.FeedbackParameters3DPubSubType.write(data.position_feedback_parameters[i0], cdr);
}
}
if (data.nullspace_command == null)
{
cdr.write_type_2(0);
}
else
{
int nullspace_command_length = data.nullspace_command.Count;
cdr.write_type_2(nullspace_command_length);
for (int i0 = 0; i0 < nullspace_command_length; i0++)
{
halodi_msgs.msg.JointNullSpaceConfigurationPubSubType.write(data.nullspace_command[i0], cdr);
}
}
}
public static int getCdrSerializedSize(halodi_msgs.msg.TaskSpaceCommand data)
{
return(getCdrSerializedSize(data, 0));
}
public static void Copy(halodi_msgs.msg.TaskSpaceCommand src, halodi_msgs.msg.TaskSpaceCommand target)
{
target.Set(src);
}
public static void read(halodi_msgs.msg.TaskSpaceCommand data, Halodi.CDR.CDRDeserializer cdr)
{
data.body_frame = halodi_msgs.msg.ReferenceFrameNamePubSubType.Create();
halodi_msgs.msg.ReferenceFrameNamePubSubType.read(data.body_frame, cdr);
data.expressed_in_frame = halodi_msgs.msg.ReferenceFrameNamePubSubType.Create();
halodi_msgs.msg.ReferenceFrameNamePubSubType.read(data.expressed_in_frame, cdr);
data.express_in_z_up = cdr.read_type_7();
data.pose = geometry_msgs.msg.PosePubSubType.Create();
geometry_msgs.msg.PosePubSubType.read(data.pose, cdr);
data.angular_velocity = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.read(data.angular_velocity, cdr);
data.linear_velocity = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.read(data.linear_velocity, cdr);
data.angular_acceleration = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.read(data.angular_acceleration, cdr);
data.linear_acceleration = geometry_msgs.msg.Vector3PubSubType.Create();
geometry_msgs.msg.Vector3PubSubType.read(data.linear_acceleration, cdr);
int passivity_input_length = cdr.read_type_2();
data.passivity_input = new System.Collections.Generic.List <halodi_msgs.msg.PassivityData>(passivity_input_length);
for (int i = 0; i < passivity_input_length; i++)
{
halodi_msgs.msg.PassivityData new_passivity_input = halodi_msgs.msg.PassivityDataPubSubType.Create();
halodi_msgs.msg.PassivityDataPubSubType.read(new_passivity_input, cdr);
data.passivity_input.Add(new_passivity_input);
}
int orientation_feedback_parameters_length = cdr.read_type_2();
data.orientation_feedback_parameters = new System.Collections.Generic.List <halodi_msgs.msg.FeedbackParameters3D>(orientation_feedback_parameters_length);
for (int i = 0; i < orientation_feedback_parameters_length; i++)
{
halodi_msgs.msg.FeedbackParameters3D new_orientation_feedback_parameters = halodi_msgs.msg.FeedbackParameters3DPubSubType.Create();
halodi_msgs.msg.FeedbackParameters3DPubSubType.read(new_orientation_feedback_parameters, cdr);
data.orientation_feedback_parameters.Add(new_orientation_feedback_parameters);
}
int position_feedback_parameters_length = cdr.read_type_2();
data.position_feedback_parameters = new System.Collections.Generic.List <halodi_msgs.msg.FeedbackParameters3D>(position_feedback_parameters_length);
for (int i = 0; i < position_feedback_parameters_length; i++)
{
halodi_msgs.msg.FeedbackParameters3D new_position_feedback_parameters = halodi_msgs.msg.FeedbackParameters3DPubSubType.Create();
halodi_msgs.msg.FeedbackParameters3DPubSubType.read(new_position_feedback_parameters, cdr);
data.position_feedback_parameters.Add(new_position_feedback_parameters);
}
int nullspace_command_length = cdr.read_type_2();
data.nullspace_command = new System.Collections.Generic.List <halodi_msgs.msg.JointNullSpaceConfiguration>(nullspace_command_length);
for (int i = 0; i < nullspace_command_length; i++)
{
halodi_msgs.msg.JointNullSpaceConfiguration new_nullspace_command = halodi_msgs.msg.JointNullSpaceConfigurationPubSubType.Create();
halodi_msgs.msg.JointNullSpaceConfigurationPubSubType.read(new_nullspace_command, cdr);
data.nullspace_command.Add(new_nullspace_command);
}
}