Permalink
Browse files

Merge remote-tracking branch 'oneamtu/master' into kickdeciding

Conflicts:
	src/man/noggin/navigator/NavHelper.py
  • Loading branch information...
2 parents 6ad2df2 + b9bbb06 commit b5b47290c0dd0b98d391d2e898fbd363ce3b1e9c @jzalinger jzalinger committed Jun 13, 2012
View
@@ -58,7 +58,7 @@ Man::Man (RobotMemory::ptr memory,
// initialize core processing modules
#ifdef USE_MOTION
- motion = boost::shared_ptr<Motion> (new Motion(enactor, sensors, pose));
+ motion = boost::shared_ptr<Motion> (new Motion(enactor, sensors, pose, memory->get<MMotion>()));
guardian->setMotionInterface(motion->getInterface());
#endif
// initialize python roboguardian module.
@@ -129,6 +129,10 @@ void Man::stopSubThreads() {
cout << "Man stopping: " << endl;
#endif
+ //remove stiffnesses
+ cout << "Killing stiffnesses " << endl;
+ motion->getInterface()->sendFreezeCommand(FreezeCommand::ptr(new FreezeCommand()));
+
loggingBoard->reset();
guardian->stop();
@@ -10,6 +10,7 @@
#include "protos/Vision.pb.h"
#include "protos/Loc.pb.h"
#include "protos/Sensors.pb.h"
+#include "protos/Motion.pb.h"
#include "MemoryCommon.h"
#include "ClassHelper.h"
@@ -35,6 +36,7 @@ ADD_MEMORY_OBJECT(MLocalization, PLoc)
ADD_MEMORY_OBJECT(MRawNaoImages, PRawNaoImages)
ADD_MEMORY_OBJECT(MVisionSensors, PVisionSensors)
ADD_MEMORY_OBJECT(MMotionSensors, PMotionSensors)
+ADD_MEMORY_OBJECT(MMotion, Motion)
//adds quick camera methods to MRawNaoImages
class MRawImages : public MRawNaoImages {
@@ -18,6 +18,7 @@ RobotMemory::RobotMemory() {
this->addObject<MMotionSensors>();
this->addObject<MRawImages>();
this->addObject<MLocalization>();
+ this->addObject<MMotion>();
}
RobotMemory::~RobotMemory() {
@@ -25,10 +25,11 @@ static const std::string streamableObjects[] = {
"MRawImages",
"MVisionSensors",
"MMotionSensors",
- "MLocalization"
+ "MLocalization",
+ "MMotion"
};
-static const int numStreamableObjects = 5;
+static const int numStreamableObjects = 6;
class RobotMemory : public Memory {
@@ -23,7 +23,8 @@ set(PROTO_INPUT Sensors.proto
Vision.proto
Loc.proto
Common.proto
- Ball.proto)
+ Ball.proto
+ Motion.proto)
#protobuf compiler
set(PROTOC_C_OUT_FLAG --cpp_out)
@@ -0,0 +1,38 @@
+package man.memory.proto;
+
+import "Common.proto";
+
+message WalkProvider {
+
+ optional bool active = 1;
+ optional bool stopping = 3;
+ optional bool requested_to_stop = 4;
+
+ optional BHDebug bhdebug = 5;
+
+ message BHDebug {
+
+ optional uint32 motion_type = 1;
+
+ }
+}
+
+message ScriptedProvider {
+
+ optional bool active = 1;
+ optional bool stopping = 2;
+}
+
+message Motion {
+
+ optional int64 timestamp = 1;
+
+ optional string current_body_provider = 2;
+ optional string next_body_provider = 3;
+
+ optional RobotLocation odometry = 4;
+
+ optional WalkProvider walk_provider = 5;
+ optional ScriptedProvider scripted_provider = 6;
+
+}
@@ -30,9 +30,10 @@ using namespace std;
Motion::Motion (boost::shared_ptr<MotionEnactor> _enactor,
boost::shared_ptr<Sensors> s,
- boost::shared_ptr<NaoPose> _pose)
+ boost::shared_ptr<NaoPose> _pose,
+ man::memory::MMotion::ptr mMotion)
: Thread("Motion"),
- switchboard(s, _pose),
+ switchboard(s, _pose, mMotion),
interface(&switchboard),
enactor(_enactor),
pose(_pose)
View
@@ -47,12 +47,15 @@
#include "NaoPose.h"
#include "synchro/synchro.h"
+#include "memory/MObjects.h"
+
class Motion : public Thread
{
public:
Motion(boost::shared_ptr<MotionEnactor> _enactor,
boost::shared_ptr<Sensors> s,
- boost::shared_ptr<NaoPose> _pose);
+ boost::shared_ptr<NaoPose> _pose,
+ man::memory::MMotion::ptr mMotion = man::memory::MMotion::ptr());
virtual ~Motion();
int start();
@@ -53,6 +53,7 @@ class MotionInterface
void enqueue(const BodyJointCommand::ptr command);
void enqueue(const HeadJointCommand::ptr command);
inline bool isWalkActive() {return switchboard->isWalkActive();}
+ inline bool isStanding() {return switchboard->isStanding();}
inline bool isHeadActive(){return switchboard->isHeadActive();}
inline bool isBodyActive(){return switchboard->isBodyActive();}
void setGait(const Gait::ptr command);
@@ -12,7 +12,8 @@ using namespace NBMath;
//#define DEBUG_SWITCHBOARD
MotionSwitchboard::MotionSwitchboard(boost::shared_ptr<Sensors> s,
- boost::shared_ptr<NaoPose> pose)
+ boost::shared_ptr<NaoPose> pose,
+ MemoryMotion::ptr mMotion)
: sensors(s),
walkProvider(sensors, pose),
scriptedProvider(sensors),
@@ -31,7 +32,8 @@ MotionSwitchboard::MotionSwitchboard(boost::shared_ptr<Sensors> s,
newJoints(false),
newInputJoints(false),
readyToSend(false),
- noWalkTransitionCommand(true)
+ noWalkTransitionCommand(true),
+ memoryProvider(&MotionSwitchboard::updateMemory, this, mMotion)
{
//Allow safe access to the next joints
@@ -124,6 +126,7 @@ void MotionSwitchboard::run() {
processStiffness();
bool active = postProcess();
+ memoryProvider.updateMemory();
if(active)
{
@@ -374,6 +377,9 @@ void MotionSwitchboard::preProcessBody()
if (curProvider != &nullBodyProvider &&
nextProvider == &nullBodyProvider)
{
+ #ifdef DEBUG_SWITCHBOARD
+ cout << "Hard reset on "<< *curProvider <<endl;
+ #endif
scriptedProvider.hardReset();
walkProvider.hardReset();
}
@@ -517,6 +523,9 @@ void MotionSwitchboard::swapBodyProvider(){
}
}
curProvider = nextProvider;
+ #ifdef DEBUG_SWITCHBOARD
+ cout << "Switched to " << *curProvider << " from "<< old_provider << endl;
+ #endif
break;
case NULL_PROVIDER:
@@ -806,6 +815,27 @@ void MotionSwitchboard::sendMotionCommand(const DestinationCommand::ptr command)
pthread_mutex_unlock(&next_provider_mutex);
}
+void MotionSwitchboard::updateMemory(MemoryMotion::ptr mMotion) const {
+
+ proto::Motion* proto_motion = mMotion->get();
+
+ proto_motion->set_current_body_provider(curProvider->getName());
+ proto_motion->set_next_body_provider(nextProvider->getName());
+
+ proto::RobotLocation* odometry = proto_motion->mutable_odometry();
+
+ MotionModel motionOdometry = getOdometryUpdate();
+ odometry->set_h(motionOdometry.theta);
+ odometry->set_x(motionOdometry.x);
+ odometry->set_y(motionOdometry.y);
+
+ walkProvider.update(proto_motion->mutable_walk_provider());
+
+ proto::ScriptedProvider* scripted_provider = proto_motion->mutable_scripted_provider();
+ scripted_provider->set_active(scriptedProvider.isActive());
+ scripted_provider->set_stopping(scriptedProvider.isStopping());
+}
+
//vector<float> MotionSwitchboard::getBodyJointsFromProvider(MotionProvider* provider) {
//
// vector<float> joints(NUM_JOINTS, 0);
@@ -58,16 +58,24 @@
#include "SetHeadCommand.h"
#include "CoordHeadCommand.h"
+#include "memory/MObjects.h"
+#include "memory/MemoryProvider.h"
+
#ifdef DEBUG_MOTION
# define DEBUG_JOINTS_OUTPUT
#endif
using namespace man::motion;
class MotionSwitchboard : public MotionSwitchboardInterface {
+
+ typedef man::memory::MMotion MemoryMotion;
+ typedef man::memory::MemoryProvider<MemoryMotion, MotionSwitchboard> MemoryProvider;
+
public:
MotionSwitchboard(boost::shared_ptr<Sensors> s,
- boost::shared_ptr<NaoPose> pose);
+ boost::shared_ptr<NaoPose> pose,
+ MemoryMotion::ptr mMotion = MemoryMotion::ptr());
~MotionSwitchboard();
void start();
@@ -94,14 +102,15 @@ class MotionSwitchboard : public MotionSwitchboardInterface {
curProvider->requestStop();
}
- bool isWalkActive(){return walkProvider.isWalkActive();}
+ bool isWalkActive(){return walkProvider.isActive();}
+ bool isStanding() {return walkProvider.isStanding();}
bool isHeadActive(){return headProvider.isActive();}
bool isBodyActive(){return curProvider->isActive();}
void resetWalkProvider(){ walkProvider.hardReset(); }
void resetScriptedProvider(){ scriptedProvider.hardReset(); }
- MotionModel getOdometryUpdate(){
+ MotionModel getOdometryUpdate() const {
return walkProvider.getOdometryUpdate();
}
@@ -130,6 +139,8 @@ class MotionSwitchboard : public MotionSwitchboardInterface {
static std::vector<float> getBodyJointsFromProvider(MotionProvider* provider);
std::vector<BodyJointCommand::ptr> generateNextBodyProviderTransitions();
+ void updateMemory(MemoryMotion::ptr mMotion) const;
+
#ifdef DEBUG_JOINTS_OUTPUT
void initDebugLogs();
void closeDebugLogs();
@@ -171,6 +182,9 @@ class MotionSwitchboard : public MotionSwitchboardInterface {
bool noWalkTransitionCommand;
+ MemoryMotion::ptr mMotion;
+ MemoryProvider memoryProvider;
+
#ifdef DEBUG_JOINTS_OUTPUT
FILE* joints_log;
FILE* stiffness_log;
@@ -121,8 +121,8 @@ BOOST_PYTHON_MODULE(_motion)
;
class_<PyStepCommand, bases<PyMotionCommand> >("StepCommand",
- init<float, float, float, int>(args("x","y","theta",
- "numSteps"),
+ init<float, float, float, float>(args("x","y","theta",
+ "gain"),
"A container for a step command. ""Holds an x, y and theta which represents a"
" walk vector, in addition to the number of desired steps."))
;
@@ -152,6 +152,7 @@ BOOST_PYTHON_MODULE(_motion)
.def("isWalkActive", &PyMotionInterface::isWalkActive)
.def("isHeadActive", &PyMotionInterface::isHeadActive)
.def("isBodyActive", &PyMotionInterface::isBodyActive)
+ .def("isStanding", &PyMotionInterface::isStanding)
.def("stopBodyMoves", &PyMotionInterface::stopBodyMoves)
.def("stopHeadMoves", &PyMotionInterface::stopHeadMoves)
.def("calibrated", &PyMotionInterface::calibrated)
@@ -189,14 +189,14 @@ class PyDestinationCommand : public PyMotionCommand {
class PyStepCommand : public PyMotionCommand {
public:
- PyStepCommand(float x_cms, float m_cms, float theta_degs, int numStep) {
+ PyStepCommand(float x_cms, float m_cms, float theta_degs, float gain=1.0f) {
//All python units should be in CM and DEG per second
//C++ is in mm and rads, so we need to convert
command =
StepCommand::ptr(new StepCommand(x_cms*CM_TO_MM,
m_cms*CM_TO_MM,
theta_degs*TO_RAD,
- numStep));
+ gain));
}
StepCommand::ptr getCommand() const {
@@ -377,6 +377,10 @@ class PyMotionInterface {
return motionInterface->isBodyActive();
}
+ bool isStanding() {
+ return motionInterface->isStanding();
+ }
+
void resetWalkProvider(){
motionInterface->resetWalkProvider();
}
@@ -7,17 +7,17 @@ class StepCommand : public MotionCommand {
public:
typedef boost::shared_ptr<StepCommand> ptr;
- StepCommand(float _x_mms, float _y_mms, float _theta_rads, int nSteps = 1)
- : MotionCommand(MotionConstants::WALK),
- x_mms(_x_mms),y_mms(_y_mms),theta_rads(_theta_rads), numSteps(nSteps)
+ StepCommand(float _x_mms, float _y_mms, float _theta_rads, float gain = 1.0f)
+ : MotionCommand(MotionConstants::STEP),
+ x_mms(_x_mms),y_mms(_y_mms),theta_rads(_theta_rads), gain(gain)
{ setChainList(); }
~StepCommand(){}
public:
// velocities
const float x_mms; //mm/second
const float y_mms; //mm/second
const float theta_rads; //rad/second
- const int numSteps;
+ const float gain;
protected:
virtual void setChainList() {
@@ -31,7 +31,7 @@ class StepCommand : public MotionCommand {
{
return o << "StepCommand("
<< w.x_mms << "," << w.y_mms << ","
- << w.theta_rads << ",nSteps=" <<w.numSteps
+ << w.theta_rads << ", gain=" <<w.gain
<< ") ";
}
};
@@ -1243,12 +1243,15 @@ void StepGenerator::resetQueues(){
* it is vital to call 'resetOdometry()' in order to make sure any movement
* since the last call to getOdometryUpdate doesnt get lost
*/
-vector<float> StepGenerator::getOdometryUpdate(){
+vector<float> StepGenerator::getOdometryUpdate() const {
const float rotation = -safe_asin(cc_Transform(1,0));
const ufvector3 odo = prod(cc_Transform,CoordFrame3D::vector3D(0.0f,0.0f));
const float odoArray[3] = {odo(0),odo(1),rotation};
//printf("Odometry update is (%g,%g,%g)\n",odoArray[0],odoArray[1],odoArray[2]);
- cc_Transform = CoordFrame3D::translation3D(0.0f,0.0f);
+
+ // NOTE: commented out because it breaks const correctness
+ // HACK HACK HACK
+// cc_Transform = CoordFrame3D::translation3D(0.0f,0.0f);
return vector<float>(odoArray,&odoArray[3]);
}
@@ -136,7 +136,7 @@ class StepGenerator {
void takeSteps(const float _x, const float _y, const float _theta,
const int _numSteps);
- std::vector <float> getOdometryUpdate();
+ std::vector <float> getOdometryUpdate() const;
void resetHard();
Oops, something went wrong.

0 comments on commit b5b4729

Please sign in to comment.