public void Start()
{
base_footprint = new GeometryPoseStamped();
torso_lift_link = new GeometryTransformStamped();
hand_camera_frame = new GeometryPose();
Invoke("Init", 1.0f);
}
void GetGoal(Message message)
{
MoveBaseActionGoal moveBaseActionGoal = (MoveBaseActionGoal)message;
goal = moveBaseActionGoal.goal.target_pose;
check_topic = true;
}
public void SendPoseStamped(Pose pose)
{
GeometryPoseStamped data = new GeometryPoseStamped();
data.pose.position.x = (float)pose.Position.X;
data.pose.position.y = (float)pose.Position.Y;
data.pose.position.z = 0;
double theta = pose.AngleW;
data.pose.orientation.z = (float)Math.Sin(theta / 2);
data.pose.orientation.w = (float)Math.Cos(theta / 2);
this.Publish(paramsRosSocket.publication_robotnavigation, data);
}
private static void gazeboLinkStatesMessage2(Message message)
{
GeometryPoseStamped geo = (GeometryPoseStamped)message;
currentPosition.pose.pose.position = geo.pose.position;
currentPosition.pose.pose.orientation = geo.pose.orientation;
if ((Math.Pow(currentPosition.pose.pose.position.x - nextWaypoint.pose.pose.position.x, 2) + Math.Pow(currentPosition.pose.pose.position.y - nextWaypoint.pose.pose.position.y, 2)) < (Math.Pow(radius, 2)))
{
if (wpToReachList.Count > 0)
{
wpToReachList.RemoveAt(0);
}
passedWaypoint = nextWaypoint;
// Console.WriteLine("In radius, passed waypoint {0} ", passedWaypoint.pose.pose.position.x);
Result(true);
isWaypoinPassed = true;
//tokenSource2.Cancel();
}
else
{
if ((Math.Pow(currentPosition.pose.pose.position.x - nextTempWaypoint.pose.pose.position.x, 2) + Math.Pow(currentPosition.pose.pose.position.y - nextTempWaypoint.pose.pose.position.y, 2)) < (Math.Pow(radius, 2)))
{
if (wpToReachList.Count > 0)
{
wpToReachList.RemoveAt(0);
}
passedWaypoint = nextTempWaypoint;
Result(true);
isWaypoinPassed = true;
}
else
{
isWaypoinPassed = false;
}
}
}
private void UpdatePosition(Message message)
{
base_footprint = (GeometryPoseStamped)message;
}
