LGD/hand_control
1
0
Fork 0
Code Issues Pull requests Releases Wiki Activity

comments commander.cpp

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-06-26 20:13:29 +02:00
parent 16e9dae1a7
commit 8e0ec310d4
1 changed files with 24 additions and 25 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

47
src/commander.cpp
View file

@ -19,38 +19,34 @@
class Run
{
private:
float xx, yy, zz, theta;
// xx < 0 : forward
// xx > 0 : backward
float xx, yy, zz, theta; // read coords
// yy > 0 : right
// yy < 0 : left
// zz > 0 : up
// zz < 0 : down
float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z;
float max_curv;
float z_dev_min, x_dev_min, y_dev_min, th_dev_min;
uint64_t min_number;
bool no_diag;
// see README.md to know what are the parameters
float x_vel, y_vel, z_vel, angle_vel, up_factor, neutral_z; // parameters
float max_curv; // not used yet
float z_dev_min, x_dev_min, y_dev_min, th_dev_min; // parameters : thresholds
uint64_t min_number; // parameter
bool no_diag; // parameter
ros::Publisher pub;
void publish()
// build and publish a message from the "xx", "yy", "zz" and "theta" informations
{
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
if (fabs(zz) > z_dev_min)
{
// up_factor to balance out the gravity effect
if (zz > 0)
mvt->linear.z = zz * z_vel * up_factor ;
else
mvt->linear.z = zz * z_vel;
}
// no_diag true : the drone can only translate on the "x" axis
// or the "y" axis but not on a linear combination of these axes.
if (no_diag)
{
if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
@ -61,7 +57,7 @@ class Run
{
mvt->linear.x = - xx * x_vel;
}
} else
} else // no_diag false : the drone can translate on any possible direction
{
if (fabs(yy) > y_dev_min)
mvt->linear.y = yy * y_vel;
@ -73,21 +69,20 @@ class Run
mvt->angular.z = theta * angle_vel;
}
assert(mvt->linear.x == 0. || mvt->linear.y == 0.);
pub.publish(mvt);
ROS_INFO("cmd published");
}//end publish
}
public:
Run(const ros::Publisher& cmd_publisher) :
pub(cmd_publisher)
{
}
pub(cmd_publisher) {}
void callback(const hand_control::Plan::ConstPtr& msg)
// handle received messages
{
ROS_INFO("plan received");
if (msg->curvature < max_curv && msg->number > min_number)
// we ever have msg->curvature == 0 in fact (not implemented yet)
{
if(msg->normal.z > 0)
@ -113,7 +108,9 @@ class Run
publish();
};
void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level) {
void reconfigure(const hand_control::CommanderConfig& c, const uint32_t& level)
// updates the parameters (received with dynamic_reconfigure)
{
max_curv = c.max_curvature;
x_dev_min = c.x_minimal_deviation;
y_dev_min = c.y_minimal_deviation;
@ -130,6 +127,7 @@ class Run
}
void run()
// runs the callbacks and publications process
{
ros::spin();
}
@ -140,16 +138,17 @@ int main(int argc, char** argv)
{
ros::init(argc, argv, "commander");
ros::NodeHandle node("commander");
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
// setting up dynamic_reconfigure (for rqt_reconfigure)
dynamic_reconfigure::Server<hand_control::CommanderConfig> server;
dynamic_reconfigure::Server<hand_control::CommanderConfig>::CallbackType f;
f = boost::bind(&Run::reconfigure, &run, _1, _2);
server.setCallback(f);
// starts working
run.run();
return 0;
}