Skip to content

SimpleController: fix sending commands without having any input samples #2

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

Open
D-Alex wants to merge 3 commits into
base: master
Choose a base branch
from
Open

SimpleController: fix sending commands without having any input samples #2

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
d1d51f6
SimpleController: move init cmd to configureHook
Nov 30, 2015
5fe12b7
SimpleController: fix sending commands without input samples
Nov 30, 2015
c6ec939
update to new naming scheme
Dec 2, 2015
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
9 changes: 6 additions & 3 deletions skid4_control.orogen
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,6 +26,9 @@ task_context "SimpleController" do

needs_configuration

property("timeout", "double",0.1).
doc("Timeout in seconds after a stop command is emitted if no new motion commands are available.")

property("wheel_radius", "double").
doc("the radius of a wheel, in meters")
property("track_width", "double").
Expand All @@ -34,7 +37,7 @@ task_context "SimpleController" do
property("left_wheel_names", "std::vector<std::string>")
property("right_wheel_names", "std::vector<std::string>")

input_port('motion_command', 'base/MotionCommand2D').
input_port('motion_command', 'base/commands/Motion2D').
doc('input command in rotation and translation. Requires a data connection type.')
end

Expand All @@ -48,7 +51,7 @@ task_context "ConstantSpeedController" do
property("speed", "double", 0.0).
doc("speed that is applied to the motors")

input_port('motion_command', 'base/MotionCommand2D').
input_port('motion_command', 'base/commands/Motion2D').
doc('input command in rotation and translation. Requires a data connection type.')
end

Expand All @@ -75,7 +78,7 @@ task_context "SoftTurnController" do
property("Tt", "double", -1.0)


input_port('motion_command', 'base/MotionCommand2D').
input_port('motion_command', 'base/commands/Motion2D').
doc('input command in rotation and translation. Requires a data connection type.')
end

Expand Down
51 changes: 31 additions & 20 deletions tasks/SimpleController.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,9 @@ bool SimpleController::configureHook()
m_rightIndexes.push_back(curIndex);
curIndex++;
}


// The output of this controller is a speed command.
m_cmd.mode[0] = m_cmd.mode[1] = m_cmd.mode[2] = m_cmd.mode[3] = base::actuators::DM_SPEED;
return true;
}

Expand All @@ -59,36 +61,45 @@ void SimpleController::updateHook()

// This is the user's command
base::commands::Motion2D cmd_in;
if (_motion_command.readNewest(cmd_in) == RTT::NoData)
switch(_motion_command.readNewest(cmd_in))
{
// no data connection
case RTT::NoData:
// send stop command
cmd_in.translation = 0;
cmd_in.rotation = 0;
}
cmd_in.rotation = 0;
break;

// The output of this controller is a speed command.
m_cmd.mode[0] = m_cmd.mode[1] =
m_cmd.mode[2] = m_cmd.mode[3] = base::actuators::DM_SPEED;
// old data
case RTT::OldData:
if((base::Time::now()-m_cmd.time).toSeconds() > _timeout.value())
{
// send stop command
cmd_in.translation = 0;
cmd_in.rotation = 0;
}
break;

// new data are available
case RTT::NewData:
// set timestamp
m_cmd.time = base::Time::now();
m_jointCmd.time = m_cmd.time;
break;
}

double fwd_velocity = cmd_in.translation / m_radius;
double differential = cmd_in.rotation * m_trackWidth / m_radius;
m_cmd.target[base::actuators::WHEEL4_FRONT_LEFT] = fwd_velocity - differential;
m_cmd.target[base::actuators::WHEEL4_REAR_LEFT] = fwd_velocity - differential;
m_cmd.target[base::actuators::WHEEL4_FRONT_RIGHT] = fwd_velocity + differential;
m_cmd.target[base::actuators::WHEEL4_REAR_RIGHT] = fwd_velocity + differential;
m_cmd.time = base::Time::now();

_simple_command.write(m_cmd);

for(std::vector<size_t>::const_iterator it = m_leftIndexes.begin(); it != m_leftIndexes.end();it++)
{
for(std::vector<size_t>::const_iterator it = m_leftIndexes.begin(); it != m_leftIndexes.end();++it)
m_jointCmd.elements[*it].speed = fwd_velocity - differential;
}
for(std::vector<size_t>::const_iterator it = m_rightIndexes.begin(); it != m_rightIndexes.end();it++)
{
for(std::vector<size_t>::const_iterator it = m_rightIndexes.begin(); it != m_rightIndexes.end();++it)
m_jointCmd.elements[*it].speed = fwd_velocity + differential;
}

m_jointCmd.time = m_cmd.time;


// write data to output ports
_simple_command.write(m_cmd);
_command.write(m_jointCmd);
}