Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

[WIP] TimeOut feature #14

Open
wants to merge 1 commit into
base: master
Choose a base branch
from
Open

[WIP] TimeOut feature #14

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
45 changes: 35 additions & 10 deletions tasks/Task.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,6 +39,8 @@ std::string Task::printState(const TaskBase::States& state)
return "TURN_ON_SPOT";
case STABILITY_FAILED:
return "STABILITY_FAILED";
case NO_POSITION_UPDATES:
return "NO_POSITION_UPDATES";
default:
return "UNKNOWN_STATE";
}
Expand Down Expand Up @@ -70,27 +72,40 @@ bool Task::startHook()

if (! TaskBase::startHook())
return false;
_robot2map.registerUpdateCallback([this](const base::Time& time){lastPoseUpdate = time;});
return true;
}
void Task::updateHook()
{
TaskBase::updateHook();

motionCommand.translation = 0;
motionCommand.rotation = 0;
motionCommand.heading = 0;
Motion2D motionCommand{};

base::Time now;
if(_simulated_time.connected())
{
if(_simulated_time.readNewest(now)==RTT::NoData)
{
LOG_ERROR_S << "Connected to simulated time, but got no time input. Aborting update.\n";
return;
}
}
else
{
now = base::Time::now();
}

Eigen::Affine3d robot2map;
if(!_robot2map.get(base::Time::now(), robot2map, false))
if(!_robot2map.get(lastPoseUpdate, robot2map, false))
{
LOG_ERROR_S << "Could not get robot pose!" << std::endl;
trajectoryFollower.removeTrajectory();
_motion_command.write(motionCommand.toBaseMotion2D());
state(SLAM_POSE_INVALID);
return;
}

base::Pose robotPose(robot2map);

if (_trajectory.readNewest(trajectories, false) == RTT::NewData && !trajectories.empty()) {
trajectoryFollower.setNewTrajectory(trajectories.front(), robotPose);
_current_trajectory.write(trajectoryFollower.getData().currentTrajectory);
Expand Down Expand Up @@ -155,7 +170,17 @@ void Task::updateHook()
}

_follower_data.write(trajectoryFollower.getData());


base::Time const latency = now - lastPoseUpdate;
if(latency > base::Time::fromSeconds(_transformer_max_latency.value())){
LOG_ERROR_S << "No position updates since " << lastPoseUpdate << " (" << latency.toSeconds() << "s ago)\n";
new_state = NO_POSITION_UPDATES;
if(_send_zero_cmd_on_timeout)
{
motionCommand = Motion2D{};
}
}


if ( not ( isMotionCommandZero(lastMotionCommand) &&
isMotionCommandZero(motionCommand) &&
Expand All @@ -178,13 +203,13 @@ void Task::updateHook()
void Task::errorHook()
{
TaskBase::errorHook();
Motion2D motionCommand{};
_motion_command.write(motionCommand.toBaseMotion2D());
}

void Task::stopHook()
{
motionCommand.translation = 0;
motionCommand.rotation = 0;
motionCommand.heading = 0;
Motion2D motionCommand{};
_motion_command.write(motionCommand.toBaseMotion2D());

TaskBase::stopHook();
Expand All @@ -207,4 +232,4 @@ bool Task::cancelCurrentTrajectory()
}
trajectories.clear();
return true;
}
}
8 changes: 4 additions & 4 deletions tasks/Task.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,10 +30,10 @@ namespace trajectory_follower{
protected:
std::vector<SubTrajectory> trajectories;
TrajectoryFollower trajectoryFollower;
Motion2D motionCommand;
Motion2D lastMotionCommand;
States new_state;
States current_state;
States new_state = PRE_OPERATIONAL;
States current_state = PRE_OPERATIONAL;
base::Time lastPoseUpdate;

std::string printState(const States& state);
bool isMotionCommandZero(const Motion2D& mc);
Expand All @@ -55,7 +55,7 @@ namespace trajectory_follower{

/** Default deconstructor of Task
*/
~Task();
~Task();

/** This hook is called by Orocos when the state machine transitions
* from PreOperational to Stopped. If it returns false, then the
Expand Down
7 changes: 6 additions & 1 deletion trajectory_follower.orogen
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -17,12 +17,17 @@ task_context "Task" do

property("send_zero_cmd_once", "bool", false).
doc "Don't send zero motion_commands repetitively"
property("send_zero_cmd_on_timeout", "bool", false).
doc "If no pose update arrived within `max_latency`, send a zero command to make the robot stop. By default, no command is send"

# Input ports
input_port("trajectory", "std::vector<trajectory_follower/SubTrajectory>").
doc("Trajectory the robot should follow").
needs_buffered_connection

input_port("simulated_time", "base::Time").
doc "When running on simulation or log data, you can connect this to a port which outputs the simulated current time. If this port is unconnected base::Time::now() is used."

# Transformations
transformer do
transform "robot", "map"
Expand All @@ -46,7 +51,7 @@ task_context "Task" do
doc("Stops following the current trajectory. Will start following the next incoming trajectory")

# Runtime state for when trajectory finished or when actively following one
runtime_states :FINISHED_TRAJECTORIES, :FOLLOWING_TRAJECTORY, :TURN_ON_SPOT, :LATERAL, :SLAM_POSE_INVALID
runtime_states :FINISHED_TRAJECTORIES, :FOLLOWING_TRAJECTORY, :TURN_ON_SPOT, :LATERAL, :SLAM_POSE_INVALID, :NO_POSITION_UPDATES

# Runtime error state entered when the initial stability test failed for a
# particular trajectory. Note that the component might switch back to
Expand Down