Go to the documentation of this file.00001 #ifndef ROBOTCONTROLLER_H
00002 #define ROBOTCONTROLLER_H
00003
00004 #include <map>
00005 #include <string>
00006 #include <vector>
00007
00008 #include "common/PID.hh"
00009 #include "common/Time.hh"
00010 #include "physics/PhysicsTypes.hh"
00011
00012 #include "JointController.h"
00013
00014 #include <boost/thread.hpp>
00015
00016 #include <ros/ros.h>
00017
00018 namespace gazebo
00019 {
00020 class RobotController
00021 {
00022 public:
00023 RobotController(physics::ModelPtr _modelPtr);
00024 ~RobotController();
00025
00026
00027 void addJoint(physics::JointPtr _jointPtr, bool advancedMode = false);
00028
00029
00030 void addJointDependency(std::string _childName, std::string _parentName, double factor);
00031 void setPosPid(const std::string& name, double _p = 0.0, double _i = 0.0, double _d = 0.0,
00032 double _imax = 0.0, double _imin = 0.0,
00033 double _cmdMax = 0.0, double _cmdMin = 0.0);
00034 void setVelPid(const std::string& name, double _p = 0.0, double _i = 0.0, double _d = 0.0,
00035 double _imax = 0.0, double _imin = 0.0,
00036 double _cmdMax = 0.0, double _cmdMin = 0.0);
00037
00038
00039 void setJointPositions(std::map<std::string, double> posMap);
00040
00041
00042 void setJointPosTarget(const std::string& name, double target);
00043 void setJointVelTarget(const std::string& name, double target);
00044 void setJointEffortTarget(const std::string& name, double target);
00045
00046
00047 void setJointControl(const r2_msgs::JointControl::ConstPtr& msg);
00048 const r2_msgs::JointStatus& getJointStatus(const std::string& name) const;
00049 void publishJointStatuses(ros::Publisher& rosPub) const;
00050
00051
00052 void update();
00053
00054 private:
00055 physics::ModelPtr modelPtr;
00056 std::map<std::string, JointControllerPtr> joints;
00057
00058 typedef std::multimap<std::string, std::pair<std::string, double> > dependenciesType;
00059 dependenciesType dependencies;
00060
00061 common::Time prevUpdateTime;
00062 };
00063 }
00064 #endif