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

adds up_factor

Browse source
This commit is contained in:
Louis-Guillaume DUBOIS 2015-05-22 17:09:29 +02:00
parent 8504825cb3
commit 49dd9208dd
2 changed files with 24 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

4
launch/all-param.launch
View file

@ -2,10 +2,10 @@
<launch> <launch>
<param name="/filtre/zmax" value="1.9" type="double"/> <param name="/filtre/zmax" value="1.9" type="double"/>
<!-- hand color
<param name="/filtre/hue" value="0.0" type="double"/> <param name="/filtre/hue" value="0.0" type="double"/>
--> <!--
<param name="/filtre/hue" value="210.0" type="double"/> <param name="/filtre/hue" value="210.0" type="double"/>
-->
<param name="/filtre/delta_hue" value="20.0" type="double"/> <param name="/filtre/delta_hue" value="20.0" type="double"/>
<param name="/commande/max_curv" value="0.02" type="double" /> <param name="/commande/max_curv" value="0.02" type="double" />

27
src/commande-new-1d.cpp
View file

@ -25,7 +25,7 @@ class Run
// dz > 0 : up // dz > 0 : up
// dz < 0 : down // dz < 0 : down
float plan_vel, z_vel; float plan_vel, z_vel, up_factor;
// to calculate dz // to calculate dz
float z_current, z_previous; float z_current, z_previous;
@ -43,10 +43,15 @@ class Run
void publish() void publish()
{ {
geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist()); geometry_msgs::Twist::Ptr mvt(new geometry_msgs::Twist());
if (fabs(dz) > dz_min) if (fabs(dz) > dz_min)
{ {
mvt->linear.z = dz * z_vel; if (dz > 0)
mvt->linear.z = dz * z_vel * up_factor ;
else
mvt->linear.z = dz * z_vel;
} }
if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min) if (fabs(yy) > fabs(xx) && fabs(yy) > y_dev_min)
{ {
mvt->linear.y = yy * plan_vel; mvt->linear.y = yy * plan_vel;
@ -55,6 +60,7 @@ class Run
{ {
mvt->linear.x = xx * plan_vel; mvt->linear.x = xx * plan_vel;
} }
pub.publish(mvt); pub.publish(mvt);
ROS_INFO("cmd published"); ROS_INFO("cmd published");
}//end publish }//end publish
@ -62,7 +68,7 @@ class Run
public: public:
Run(const ros::Publisher& cmd_publisher, const float& max_curvature, const float& plan_velocity, 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& z_velocity, const float& x_minimal_deviation, const float& y_minimal_deviation,
const float& dz_minimal_difference, const int& min_points_number) : const float& dz_minimal_difference, const int& min_points_number, const float& up_fact) :
pub(cmd_publisher), pub(cmd_publisher),
plan_vel(plan_velocity), plan_vel(plan_velocity),
max_curv(max_curvature), max_curv(max_curvature),
@ -74,7 +80,8 @@ class Run
y_dev_min(y_minimal_deviation), y_dev_min(y_minimal_deviation),
dz_min(dz_minimal_difference), dz_min(dz_minimal_difference),
first_msg(true), first_msg(true),
min_number(min_points_number){ min_number(min_points_number),
up_factor(up_fact) {
z_current = z_previous = std::numeric_limits<float>::signaling_NaN(); z_current = z_previous = std::numeric_limits<float>::signaling_NaN();
t_previous.nsec = t_previous.sec = t_previous.nsec = t_previous.sec =
t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN(); t_previous.nsec = t_previous.sec = std::numeric_limits<uint32_t>::signaling_NaN();
@ -203,8 +210,18 @@ int main(int argc, char** argv)
ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min); ROS_INFO("dz_dev_min : %f (default value)", dz_dev_min);
} }
double up_fact(0);
if (node.getParam("up_fact", up_fact))
{
ROS_INFO("up_fact : %f" , up_fact);
} else {
node.setParam("up_fact", 1.5);
node.getParam("up_fact", up_fact);
ROS_INFO("up_fact : %f (default value)", up_fact);
}
ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1); ros::Publisher cmd_pub = node.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number); Run run(cmd_pub, max_curv, plan_vel, z_vel, x_dev_min, y_dev_min, dz_dev_min, min_number, up_fact);
ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run); ros::Subscriber plan_sub = node.subscribe<hand_control::Plan>("input", 1, &Run::callback, &run);
run.run(); run.run();
return 0; return 0;