# Move robot between points

I'm trying to navigate `hector_quadrotor`

through some points in the 3D space.

I based my implementation on this github project that moves from A to B and uses `action`

to do so.

The server side uses the logic proposed on the github project above.
To move the drone, first its high is adjusted, then it rotates on z-axis and then move forward on x. Whether , `dist < 0.1`

the drone reached its pose.

The code is as follows:

```
void NavigationServer::broadcastGoalTF()
{
tf::Transform transform;
// setup transformation - relative position of goal with respect to world
transform.setOrigin( tf::Vector3(goal_point.x, goal_point.y, goal_point.z) );
transform.setRotation(tf::Quaternion(0, 0, 0, 1));
// publish transform
br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "goal"));
}
void NavigationServer::publishTwist()
{
geometry_msgs::Twist cmd;
try
{
listener_goal.lookupTransform("/base_link", "/goal",
ros::Time(0), transform_goal);
}
catch (tf::TransformException &ex)
{
//catch exception
}
// get to desired altitude
float heightDist = transform_goal.getOrigin().z();
if ( heightDist > hTresh ) {
// we need to raise up
ROS_INFO("Adjusting high...Raising up");
cmd.linear.z = 0.3;
dist = sqrt( pow(transform_goal.getOrigin().x(), 2) +
pow(transform_goal.getOrigin().y(), 2) +
pow(transform_goal.getOrigin().z(), 2)
);
} else if ( heightDist < -hTresh ) {
ROS_INFO("Adjusting high...Diving Down");
// we need to dive down
cmd.linear.z = -0.3;
dist = sqrt( pow(transform_goal.getOrigin().x(), 2) +
pow(transform_goal.getOrigin().y(), 2) +
pow(transform_goal.getOrigin().z(), 2)
);
} else if ( abs(heightDist) < hTresh ) {
// we are roughly in the same altitude as the goal so we stop any vertical movement
cmd.linear.z = 0;
ROS_INFO("Adjusting angular speed!");
// ---- set angular speed ----
cmd.angular.z = 3.0 * atan2( transform_goal.getOrigin().y(),
transform_goal.getOrigin().x());
ROS_INFO("angular_speed: %f", cmd.angular.z);
// set maximum angular speed
if ( cmd.angular.z > 1)
{
ROS_INFO("angular_speed: %f", cmd.angular.z);
cmd.angular.z = 1;
}
// if we are almost "facing" the goal
if ( abs(cmd.angular.z) < 0.3) {
ROS_INFO("Moving towards goal point (linear on x-axis)");
cmd.linear.x = 0.4 * sqrt( pow(transform_goal.getOrigin().x(), 2) +
pow(transform_goal.getOrigin().y(), 2)
);
// set maximum linear speed
if ( cmd.linear.x > 1)
{
cmd.linear.x = 1;
}
} else {
cmd.linear.x = 0;
}
dist = sqrt( pow(transform_goal.getOrigin().x(), 2) +
pow(transform_goal.getOrigin().y(), 2) +
pow(transform_goal.getOrigin().z(), 2)
);
}
cmd_vel_pub_.publish(cmd);
}
void NavigationServer::executeNavigate(const move_quadcopter::NavigateGoalConstPtr& goal)
{
ROS_INFO("move_quadcopter::navigation_server: executeNavigate");
geometry_msgs::Twist empty;
dist = 20.0;
// set goal_point
goal_point.x = goal->target_pos.x;
goal_point.y = goal->target_pos.y;
goal_point.z = goal->target_pos.z;
ros::Rate loop_rate(100);
while(ros::ok() && (dist > dTresh))
{
// broadcast static goal->world transformation
broadcastGoalTF();
// publish twist message based on robot_base -> goal transform
publishTwist();
ros::spinOnce();
loop_rate.sleep();
}
last_goal.x = goal_point.x;
last_goal.y = goal_point.y;
last_goal.z = goal_point.z;
as_.setSucceeded();
}
```

For moving from A(0,0,0) to B(1,5,3) its working fine. However, on moving from B to C(5,0,0) `dist`

is always < 0,1 and it moves to D. On gazebo its like it moves from A-B-D and C doesn't exist.
`dist`

relies on transform and ...