public void init (NodeHandle nh, string field = "")
{
NodeHandle field_nh = new NodeHandle ( nh, field );
x.init ( field_nh, "x" );
y.init ( field_nh, "y" );
z.init ( field_nh, "z" );
field_nh.param ( "max", ref absolute_maximum, double.PositiveInfinity );
field_nh.param ( "max_xy", ref absolute_maximum_xy, double.PositiveInfinity );
}
public void init (NodeHandle nh, string field = "")
{
NodeHandle field_nh = new NodeHandle ( nh, field );
roll.init ( field_nh, "roll" );
pitch.init ( field_nh, "pitch" );
field_nh.param<double> ( "max_roll_pitch", ref absolute_max, double.PositiveInfinity );
}
// public FieldLimiter (T value)
// {
// return limit ( value );
// }
public void init (NodeHandle nh, string field = "")
{
float fmin = float.NegativeInfinity;
float fmax = float.PositiveInfinity;
double dmin = double.NegativeInfinity;
double dmax = double.PositiveInfinity;
bool isDouble = typeof (T) == typeof (double);
string prefix = !string.IsNullOrEmpty ( field ) ? field + "/" : "";
if ( nh.hasParam ( prefix + "max" ) || nh.hasParam ( prefix + "min" ) )
{
object posinf = isDouble ? dmax : fmax;
object neginf = isDouble ? dmin : fmin;
nh.param<T> ( prefix + "max", ref max_, (T) posinf );
nh.param<T> ( prefix + "min", ref min_, (T) neginf );
// nh.param<T>(prefix + "max", ref max_, numeric_limits<T>::infinity());
// nh.param<T>(prefix + "min", ref min_, -max_);
ROS.Info ("Limits " + nh.getNamespace () + "/" + field + " initialized " + field + " with min " + min_ + " and max " + max_);
}
}
void OnRosInit()
{
nh = ROS.GlobalNodeHandle;
// NodeHandle privateNH = new NodeHandle("~");
// TODO dynamic reconfig
string control_mode = "";
nh.param <string>("control_mode", ref control_mode, "wrench");
// NodeHandle robot_nh = new NodeHandle ("~");
// TODO factor out
// nh.param<string>("base_link_frame", ref baseLinkFrame, "base_link");
// nh.param<string>("world_frame", ref worldFrame, "world");
// nh.param<string>("base_stabilized_frame", ref baseStabilizedFrame, "base_stabilized");
if (control_mode == "wrench")
{
wrenchPub = nh.advertise <Wrench> ("quad_rotor/cmd_force", 10);
poseSub = nh.subscribe <PoseStamped> ("quad_rotor/pose", 10, PoseCallback);
imuSub = nh.subscribe <Imu> ("quad_rotor/imu", 10, ImuCallback);
velocityPublisher = nh.advertise <Twist> ("quad_rotor/cmd_vel", 10);
}
/* else if (control_mode == "attitude")
* {
* // nh.param<double>("pitch_max", ref sAxes.x.factor, 30.0);
* // nh.param<double>("roll_max", ref sAxes.y.factor, 30.0);
* // nh.param<double>("thrust_max", ref sAxes.thrust.factor, 10.0);
* // nh.param<double>("thrust_offset", ref sAxes.thrust.offset, 10.0);
* attitudePublisher = nh.advertise<AttitudeCommand> ( "command/attitude", 10 );
* yawRatePublisher = nh.advertise<YawRateCommand> ( "command/yawrate", 10 );
* thrustPublisher = nh.advertise<ThrustCommand> ( "command/thrust", 10 );
* }
* else if (control_mode == "velocity" || control_mode == "twist")
* {
* // Gazebo uses Y=forward and Z=up
* nh.param<double>("x_velocity_max", ref xVelocityMax, 2.0);
* nh.param<double>("y_velocity_max", ref zVelocityMax, 2.0);
* nh.param<double>("z_velocity_max", ref yVelocityMax, 2.0);
*
* velocityPublisher = nh.advertise<Twist> ( "command/twist", 10 );
* }
* else if (control_mode == "position")
* {
* // Gazebo uses Y=forward and Z=up
* nh.param<double>("x_velocity_max", ref xVelocityMax, 2.0);
* nh.param<double>("y_velocity_max", ref zVelocityMax, 2.0);
* nh.param<double>("z_velocity_max", ref yVelocityMax, 2.0);
*
* pose.pose.position.x = 0;
* pose.pose.position.y = 0;
* pose.pose.position.z = 0;
* pose.pose.orientation.x = 0;
* pose.pose.orientation.y = 0;
* pose.pose.orientation.z = 0;
* pose.pose.orientation.w = 1;
* }*/
else
{
ROS.Error("Unsupported control mode: " + control_mode);
}
motorEnableService = nh.serviceClient <EnableMotors> ("enable_motors");
resetService = nh.serviceClient <SetBool.Request, SetBool.Response> ("quad_rotor/reset_orientation");
gravityService = nh.serviceClient <SetBool.Request, SetBool.Response> ("quad_rotor/gravity");
// takeoffClient = new TakeoffClient ( nh, "action/takeoff" );
// landingClient = new LandingClient ( nh, "action/landing" );
// poseClient = new PoseClient ( nh, "action/pose" );
init = true;
}
void OnRosInit()
{
nh = ROS.GlobalNodeHandle;
// NodeHandle privateNH = new NodeHandle("~");
nh.param <int>("x_axis", ref sAxes.x.axis, 5);
nh.param <int>("y_axis", ref sAxes.y.axis, 4);
nh.param <int>("z_axis", ref sAxes.z.axis, 2);
nh.param <int>("thrust_axis", ref sAxes.thrust.axis, -3);
nh.param <int>("yaw_axis", ref sAxes.yaw.axis, 1);
nh.param <double>("yaw_velocity_max", ref sAxes.yaw.factor, 90.0);
nh.param <int>("slow_button", ref sButtons.slow.button, 4);
nh.param <int>("go_button", ref sButtons.go.button, 1);
nh.param <int>("stop_button", ref sButtons.stop.button, 2);
nh.param <int>("interrupt_button", ref sButtons.interrupt.button, 3);
nh.param <double>("slow_factor", ref slowFactor, 0.2);
// TODO dynamic reconfig
string control_mode = "";
nh.param <string>("control_mode", ref control_mode, "twist");
NodeHandle robot_nh = ROS.GlobalNodeHandle;
// NodeHandle robot_nh = new NodeHandle ();
// TODO factor out
robot_nh.param <string>("base_link_frame", ref baseLinkFrame, "base_link");
robot_nh.param <string>("world_frame", ref worldFrame, "world");
robot_nh.param <string>("base_stabilized_frame", ref baseStabilizedFrame, "base_stabilized");
if (control_mode == "attitude")
{
nh.param <double>("pitch_max", ref sAxes.x.factor, 30.0);
nh.param <double>("roll_max", ref sAxes.y.factor, 30.0);
nh.param <double>("thrust_max", ref sAxes.thrust.factor, 10.0);
nh.param <double>("thrust_offset", ref sAxes.thrust.offset, 10.0);
joySubscriber = nh.subscribe <Joy> ("joy", 1, joyAttitudeCallback);
attitudePublisher = robot_nh.advertise <AttitudeCommand> ("command/attitude", 10);
yawRatePublisher = robot_nh.advertise <YawRateCommand> ("command/yawrate", 10);
thrustPublisher = robot_nh.advertise <ThrustCommand> ("command/thrust", 10);
}
else if (control_mode == "velocity")
{
nh.param <double>("x_velocity_max", ref sAxes.x.factor, 2.0);
nh.param <double>("y_velocity_max", ref sAxes.y.factor, 2.0);
nh.param <double>("z_velocity_max", ref sAxes.z.factor, 2.0);
joySubscriber = nh.subscribe <Joy> ("joy", 1, joyTwistCallback);
velocityPublisher = robot_nh.advertise <TwistStamped> ("command/twist", 10);
}
else if (control_mode == "position")
{
nh.param <double>("x_velocity_max", ref sAxes.x.factor, 2.0);
nh.param <double>("y_velocity_max", ref sAxes.y.factor, 2.0);
nh.param <double>("z_velocity_max", ref sAxes.z.factor, 2.0);
joySubscriber = nh.subscribe <Joy> ("joy", 1, joyPoseCallback);
pose.pose.position.x = 0;
pose.pose.position.y = 0;
pose.pose.position.z = 0;
pose.pose.orientation.x = 0;
pose.pose.orientation.y = 0;
pose.pose.orientation.z = 0;
pose.pose.orientation.w = 1;
}
else
{
ROS.Error("Unsupported control mode: " + control_mode);
}
motorEnableService = robot_nh.serviceClient <EnableMotors> ("enable_motors");
takeoffClient = new TakeoffClient(robot_nh, "action/takeoff");
landingClient = new LandingClient(robot_nh, "action/landing");
poseClient = new PoseClient(robot_nh, "action/pose");
}
