Skip to content

Commit f7f9b54

Browse files
saarnoldsaarnold
committed
added Task RockTutorialControl
1 parent 4b7269a commit f7f9b54

File tree

3 files changed

+238
-2
lines changed

3 files changed

+238
-2
lines changed

‎rock_tutorial.orogen‎

Lines changed: 11 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -14,11 +14,20 @@ task_context "RockTutorialVisualization" do
1414
port_driven "pose"
1515
end
1616

17-
17+
# Declare a new task context (i.e., a component)
18+
task_context "RockTutorialControl" do
19+
# Declare input port motion_command
20+
input_port "motion_command", "base::MotionCommand2D"
21+
# Declare output port pose
22+
output_port "pose", "base::Pose"
23+
end
1824

1925
# Declares a deployment, i.e. an actual executable that contains various tasks.
2026
deployment "rock_tutorial" do
21-
task("rock_tutorial", "RockTutorialVisualization")
27+
task("rock_tutorial_visualization", "RockTutorialVisualization")
2228

29+
# Declares task as periodic, with a period of 10ms
30+
task("rock_tutorial_control", "RockTutorialControl").
31+
periodic(0.01)
2332
end
2433

‎tasks/RockTutorialControl.cpp‎

Lines changed: 139 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,139 @@
1+
/* Generated from orogen/lib/orogen/templates/tasks/Task.cpp */
2+
3+
#include "RockTutorialControl.hpp"
4+
5+
using namespace rock_tutorial;
6+
7+
RockTutorialControl::RockTutorialControl(std::string const& name, TaskCore::TaskState initial_state)
8+
: RockTutorialControlBase(name, initial_state)
9+
{
10+
}
11+
12+
RockTutorialControl::RockTutorialControl(std::string const& name, RTT::ExecutionEngine* engine, TaskCore::TaskState initial_state)
13+
: RockTutorialControlBase(name, engine, initial_state)
14+
{
15+
}
16+
17+
RockTutorialControl::~RockTutorialControl()
18+
{
19+
}
20+
21+
/**
22+
* Makes sure that angles are between PI and -PI.
23+
*/
24+
void RockTutorialControl::constrainAngle(double& angle)
25+
{
26+
if (angle < -M_PI)
27+
angle = angle + 2 * M_PI;
28+
else if (angle > M_PI)
29+
angle = angle - 2 * M_PI;
30+
}
31+
32+
/**
33+
* This method constrains the relativ rotation and translation
34+
* of a 2d motion command.
35+
* Rotation should be between PI an -PI.
36+
* Translation should be between 10 and -10.
37+
*/
38+
void RockTutorialControl::constrainValues(base::MotionCommand2D& motionCommand)
39+
{
40+
if (motionCommand.rotation > M_PI)
41+
motionCommand.rotation = M_PI;
42+
else if (motionCommand.rotation < -M_PI)
43+
motionCommand.rotation = -M_PI;
44+
45+
if (motionCommand.translation > 10)
46+
motionCommand.translation = 10;
47+
else if (motionCommand.translation < -10)
48+
motionCommand.translation = -10;
49+
}
50+
51+
/// The following lines are template definitions for the various state machine
52+
// hooks defined by Orocos::RTT. See RockTutorialControl.hpp for more detailed
53+
// documentation about them.
54+
55+
// bool RockTutorialControl::configureHook()
56+
// {
57+
// if (! RockTutorialControlBase::configureHook())
58+
// return false;
59+
// return true;
60+
// }
61+
62+
bool RockTutorialControl::startHook()
63+
{
64+
// check if input ports are connected
65+
if (!_motion_command.connected())
66+
{
67+
std::cerr << TaskContext::getName() << ": "
68+
<< "Input port 'motion_command' is not connected." << std::endl;
69+
return false;
70+
}
71+
72+
// set variables to zero
73+
actualCommand.rotation = 0;
74+
actualCommand.translation = 0;
75+
taskPeriod = 0;
76+
currentHeading = 0;
77+
currentRoll = 0;
78+
currentPose.orientation = base::Quaterniond::Identity();
79+
currentPose.position = base::Vector3d::Zero();
80+
return true;
81+
}
82+
83+
void RockTutorialControl::updateHook()
84+
{
85+
// get triggering interval of this task context
86+
if (taskPeriod <= 0)
87+
{
88+
taskPeriod = TaskContext::getPeriod();
89+
if (taskPeriod <= 0)
90+
{
91+
std::cerr << TaskContext::getName() << ": "
92+
<< "This task needs to be a periodic triggering task." << std::endl;
93+
error();
94+
return;
95+
}
96+
}
97+
98+
//read new motion command if available
99+
base::MotionCommand2D motionCommand;
100+
if (_motion_command.readNewest(motionCommand) == RTT::NewData)
101+
{
102+
actualCommand = motionCommand;
103+
constrainValues(actualCommand);
104+
}
105+
106+
//translation and rotation relativ to the task period
107+
double delta_translation = actualCommand.translation * taskPeriod;
108+
double delta_rotation = actualCommand.rotation * taskPeriod;
109+
110+
// set current yaw and roll
111+
currentHeading += delta_rotation;
112+
currentRoll += delta_translation * -2;
113+
constrainAngle(currentHeading);
114+
constrainAngle(currentRoll);
115+
116+
// calculate new absolut values for position and orientation
117+
currentPose.position += Eigen::AngleAxis<double>(currentHeading, Eigen::Vector3d::UnitZ()) * Eigen::Vector3d(0, delta_translation, 0);
118+
currentPose.orientation = Eigen::AngleAxis<double>(currentHeading, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxis<double>(currentRoll, Eigen::Vector3d::UnitX());
119+
currentPose.orientation.normalize();
120+
121+
//write pose on output port
122+
if(_pose.connected())
123+
_pose.write(currentPose);
124+
}
125+
126+
// void RockTutorialControl::errorHook()
127+
// {
128+
//
129+
// }
130+
131+
// void RockTutorialControl::stopHook()
132+
// {
133+
//
134+
// }
135+
136+
// void RockTutorialControl::cleanupHook()
137+
// {
138+
//
139+
// }

‎tasks/RockTutorialControl.hpp‎

Lines changed: 88 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,88 @@
1+
/* Generated from orogen/lib/orogen/templates/tasks/Task.hpp */
2+
3+
#ifndef ROCK_TUTORIAL_ROCKTUTORIALCONTROL_TASK_HPP
4+
#define ROCK_TUTORIAL_ROCKTUTORIALCONTROL_TASK_HPP
5+
6+
#include "rock_tutorial/RockTutorialControlBase.hpp"
7+
8+
namespace rock_tutorial {
9+
class RockTutorialControl : public RockTutorialControlBase
10+
{
11+
friend class RockTutorialControlBase;
12+
protected:
13+
void constrainValues(base::MotionCommand2D& motionCommand);
14+
void constrainAngle(double& angle);
15+
16+
base::MotionCommand2D actualCommand;
17+
base::Pose currentPose;
18+
double currentHeading;
19+
double currentRoll;
20+
double taskPeriod;
21+
22+
public:
23+
RockTutorialControl(std::string const& name = "rock_tutorial::RockTutorialControl", TaskCore::TaskState initial_state = Stopped);
24+
RockTutorialControl(std::string const& name, RTT::ExecutionEngine* engine, TaskCore::TaskState initial_state = Stopped);
25+
26+
~RockTutorialControl();
27+
28+
/** This hook is called by Orocos when the state machine transitions
29+
* from PreOperational to Stopped. If it returns false, then the
30+
* component will stay in PreOperational. Otherwise, it goes into
31+
* Stopped.
32+
*
33+
* It is meaningful only if the #needs_configuration has been specified
34+
* in the task context definition with (for example):
35+
*
36+
* task_context "TaskName" do
37+
* needs_configuration
38+
* ...
39+
* end
40+
*/
41+
// bool configureHook();
42+
43+
/** This hook is called by Orocos when the state machine transitions
44+
* from Stopped to Running. If it returns false, then the component will
45+
* stay in Stopped. Otherwise, it goes into Running and updateHook()
46+
* will be called.
47+
*/
48+
bool startHook();
49+
50+
/** This hook is called by Orocos when the component is in the Running
51+
* state, at each activity step. Here, the activity gives the "ticks"
52+
* when the hook should be called.
53+
*
54+
* The error(), exception() and fatal() calls, when called in this hook,
55+
* allow to get into the associated RunTimeError, Exception and
56+
* FatalError states.
57+
*
58+
* In the first case, updateHook() is still called, and recover() allows
59+
* you to go back into the Running state. In the second case, the
60+
* errorHook() will be called instead of updateHook(). In Exception, the
61+
* component is stopped and recover() needs to be called before starting
62+
* it again. Finally, FatalError cannot be recovered.
63+
*/
64+
void updateHook();
65+
66+
/** This hook is called by Orocos when the component is in the
67+
* RunTimeError state, at each activity step. See the discussion in
68+
* updateHook() about triggering options.
69+
*
70+
* Call recovered() to go back in the Runtime state.
71+
*/
72+
//void errorHook();
73+
74+
/** This hook is called by Orocos when the state machine transitions
75+
* from Running to Stopped after stop() has been called.
76+
*/
77+
//void stopHook();
78+
79+
/** This hook is called by Orocos when the state machine transitions
80+
* from Stopped to PreOperational, requiring the call to configureHook()
81+
* before calling start() again.
82+
*/
83+
//void cleanupHook();
84+
};
85+
}
86+
87+
#endif
88+

0 commit comments

Comments
 (0)