private Vector3 GetPosition(GeometryPoseStamped message)
{
return(new Vector3(
message.pose.position.x,
message.pose.position.y,
message.pose.position.z));
}
private Quaternion GetRotation(GeometryPoseStamped message)
{
return(new Quaternion(
message.pose.orientation.x,
message.pose.orientation.y,
message.pose.orientation.z,
message.pose.orientation.w));
}
//public WaypointState wpstate;
public waypoint()
{
gid = 0;
lid = 0;
pose = new GeometryPoseStamped();
twist = new GeometryTwistStamped();
dtlane = new dtlane();
change_flag = 0;
// wpstate = new WaypointState();
}
private void Update() //毎サイクルごとにHSRのTFを習得してHSR_tf_markerを描画する
{
topic_tf = tf.base_footprint;
position = new Vector3(-topic_tf.pose.position.y, 0.5f, topic_tf.pose.position.x);
HSR_tf_marker.transform.localPosition = position;
GeometryQuaternion tmp = topic_tf.pose.orientation;
Quaternion quaternion = new Quaternion(0, tmp.z, 0, -tmp.w);
HSR_tf_marker.transform.localRotation = quaternion;
}
private void Update()
{
//Topicで指定された感情のGameObjectを表示
if (State_change)
{
Destroy(target);
target = generate_gameobject(current_state, target);
State_change = false;
}
//上で表示したGameObjectをHSRの頭部部分に移動させる
if (subscribed && target != null)
{
torso_tf = tf.torso_lift_link;
base_foot_tf = tf.base_footprint;
position.y = torso_tf.transform.translation.z + 0.21f;
position.x = -(base_foot_tf.pose.position.y + 0.02f);
position.z = base_foot_tf.pose.position.x - 0.06f;
target.transform.localPosition = position;
}
}
public MoveBaseGoal()
{
target_pose = new GeometryPoseStamped();
}
public MoveBaseFeedback()
{
base_position = new GeometryPoseStamped();
}
private void InitializeMessage()
{
message = new GeometryPoseStamped();
message.header = new StandardHeader();
message.header.frame_id = FrameId;
}
