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

commande works

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-07 23:31:31 +02:00
parent a3d8f76af6
commit 26bfafc9c9
2 changed files with 99 additions and 58 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

153
src/commande.cpp
View file

@ -2,6 +2,7 @@
#include <ros/time.h>
#include <locale.h>
#include <limits>
#include <math.h>
#include <pcl_ros/point_cloud.h>
#include <pcl/point_types.h>
@ -13,59 +14,84 @@
class Run
{
private:
ros::Rate loop_rate;
// xx*plan_vel, yy*plan_vel and dz*dz_vel will be published
double xx, yy, dz;
double plan_vel, z_vel;
float xx, yy, dz;
float plan_vel, z_vel;
// to calculate dz
double z_current, z_previous;
float z_current, z_previous;
ros::Time t_current, t_previous;
// conditions to publish a message
double max_curv;
double dz_min, x_dev_min, y_dev_min;
float max_curv;
float dz_min, x_dev_min, y_dev_min;
uint64_t min_number;
bool first_msg;
ros::Publisher pub;
void publish()
{
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
if (fabs(dz) > dz_min)
{
mvt->linear.z = dz * z_vel;
ROS_INFO("z changed");
}
if (xx > x_dev_min)
{
mvt->linear.y = xx * plan_vel;
// because of the kinect orientation
}
if (yy > y_dev_min)
{
mvt->linear.x = yy * plan_vel;
// because of the kinect orientation
}
pub.publish(mvt);
ROS_INFO("cmd published");
}//end publish
public:
Run(ros::Publisher cmd_pub, double M_curve, double p_vel, double h_vel, double x_dev_m, double y_dev_m, double dz_m) :
pub(cmd_pub),
plan_vel(p_vel),
max_curv(M_curve),
z_vel(h_vel),
loop_rate(30),
Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity,
const float& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation,
const float& dz_minimal_difference, const int& min_points_number) :
pub(cmd_publisher),
plan_vel(plan_velocity),
max_curv(max_curvature),
z_vel(z_velocity),
xx(0),
yy(0),
dz(0),
x_dev_min(x_dev_m),
y_dev_min(y_dev_m),
dz_min(dz_m),
first_msg(true) {
z_current = z_previous = std::numeric_limits<double>::signaling_NaN();
t_previous.nsec = t_previous.sec =
t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
}
x_dev_min(x_minimal_deviation),
y_dev_min(y_minimal_deviation),
dz_min(dz_minimal_difference),
first_msg(true),
loop_rate(30),
min_number(min_points_number){
z_current = z_previous = std::numeric_limits<float>::signaling_NaN();
t_previous.nsec = t_previous.sec =
t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
}
void operator()(const hand_control::Plan::ConstPtr& msg)
void callback(const hand_control::Plan::ConstPtr& msg)
{
ROS_INFO("plan received");
if (msg->curvature < max_curv)
if (msg->curvature < max_curv && msg->number > min_number)
{
t_current = msg->header.stamp;
z_current = msg->normal.z;
if (!first_msg)
{
dz = (z_current - z_previous)/((t_current - t_previous).toSec());
ROS_INFO("dz = %f", dz);
}
xx = msg->normal.x;
ROS_INFO("xx = %f", xx);
yy = msg->normal.y;
@ -73,7 +99,7 @@ class Run
t_previous = t_current;
z_previous = z_current;
z_current = std::numeric_limits<double>::signaling_NaN();
z_current = std::numeric_limits<float>::signaling_NaN();
t_current.nsec = t_current.sec = std::numeric_limits<uint32_t>::signaling_NaN();
if (first_msg)
{
@ -86,38 +112,13 @@ class Run
void run()
{
ROS_INFO("node running");
while(ros::ok())
{
ROS_INFO("loop entered");
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
ROS_INFO("dz : %f > %f : dz_min ?", dz, dz_min);
if (dz > dz_min)
{
mvt->linear.z = dz * z_vel;
ROS_INFO("z changed");
}
ROS_INFO("xx : %f > %f : x_dev_min ?", xx, x_dev_min);
if (xx > x_dev_min)
{
mvt->linear.x = xx * plan_vel;
ROS_INFO("x changed");
}
ROS_INFO("yy : %f > %f : y_dev_min ?", yy, y_dev_min);
if (yy > y_dev_min)
{
mvt->linear.y = yy * plan_vel;
ROS_INFO("y changed");
}
pub.publish(mvt);
publish();
ros::spinOnce();
loop_rate.sleep();
}//end while
}//end run
}
}
};
@ -131,7 +132,7 @@ int main(int argc, char** argv)
{
ROS_INFO("max_curv : %f" , max_curv);
} else {
node.setParam("max_curv", 0.1);
node.setParam("max_curv", 0.08);
node.getParam("max_curv", max_curv);
ROS_INFO("max_curv : %f (default value)", max_curv);
}
@ -156,9 +157,49 @@ int main(int argc, char** argv)
ROS_INFO("z_vel : %f (default value)", z_vel);
}
int min_number(0);
if (node.getParam("min_number", min_number))
{
ROS_INFO("min_number : %d" , min_number);
} else {
node.setParam("min_number", 500);
node.getParam("min_number", min_number);
ROS_INFO("min_number : %d (default value)", min_number);
}
double x_dev_min(0);
if (node.getParam("x_dev_min", z_vel))
{
ROS_INFO("x_dev_min : %f" , z_vel);
} else {
node.setParam("x_dev_min", 0.05);
node.getParam("x_dev_min", z_vel);
ROS_INFO("x_dev_min : %f (default value)", z_vel);
}
double y_dev_min(0);
if (node.getParam("y_dev_min", z_vel))
{
ROS_INFO("y_dev_min : %f" , z_vel);
} else {
node.setParam("y_dev_min", 0.05);
node.getParam("y_dev_min", z_vel);
ROS_INFO("y_dev_min : %f (default value)", z_vel);
}
double dz_dev_min(0);
if (node.getParam("dz_dev_min", z_vel))
{
ROS_INFO("dz_dev_min : %f" , z_vel);
} else {
node.setParam("dz_dev_min", 0.05);
node.getParam("dz_dev_min", z_vel);
ROS_INFO("dz_dev_min : %f (default value)", z_vel);
}
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub, max_curv, plan_vel, z_vel, 0.0, 0.0, 0.0);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, run);
Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
run.run();
return 0;
}

4
src/normal_estimator.cpp
View file

@ -43,8 +43,8 @@ class Callback {
const hand_control::Plan::ConstPtr
to_Plan(const float& x, const float& y,
const float& z, const float& h,
const float& c, uint32_t seq,
uint64_t msec64, uint64_t number)
const float& c, const uint32_t& seq,
const uint64_t& msec64, const uint64_t& number)
{
hand_control::Plan::Ptr ros_msg(new hand_control::Plan());
ros_msg->normal.x = x;