void PositionCommand::dynamic(uav_commander::PIDControlConfig &config, uint32_t level)
{
if (config.PID_Control)
{
ros::NodeHandle n_priv("~");
n_priv.param<double>("kp_x" , config.Kp_x , 0.71);
n_priv.param<double>("kp_y" , config.Kp_y , 0.72);
n_priv.param<double>("kp_z" , config.Kp_z , 0.73);
n_priv.param<double>("kp_w" , config.Kp_w , 0.74);
n_priv.param<double>("ki_x" , config.Ki_x , 0.71);
n_priv.param<double>("ki_y" , config.Ki_y , 0.72);
n_priv.param<double>("ki_z" , config.Ki_z , 0.73);
n_priv.param<double>("ki_w" , config.Ki_w , 0.74);
n_priv.param<double>("kd_x" , config.Kd_x , 0.71);
n_priv.param<double>("kd_y" , config.Kd_y , 0.72);
n_priv.param<double>("kd_z" , config.Kd_z , 0.73);
n_priv.param<double>("kd_w" , config.Kd_w , 0.74);
}
config.PID_Control = 0;
Kp << config.Kp_x , config.Kp_y , config.Kp_z , 0 , 0 , config.Kp_w;
Ki << config.Ki_x , config.Ki_y , config.Ki_z , 0 , 0 , config.Ki_w;
Kd << config.Kd_x , config.Kd_y , config.Kd_z , 0 , 0 , config.Kd_w;
control_info_msg.Kp.x = config.Kp_x ;
control_info_msg.Kp.y = config.Kp_y ;
control_info_msg.Kp.z = config.Kp_z ;
control_info_msg.Kp.r = 0 ;
control_info_msg.Kp.p = 0 ;
control_info_msg.Kp.w = config.Kp_w ;
control_info_msg.Ki.x = config.Ki_x ;
control_info_msg.Ki.y = config.Ki_y ;
control_info_msg.Ki.z = config.Ki_z ;
control_info_msg.Ki.r = 0 ;
control_info_msg.Ki.p = 0 ;
control_info_msg.Ki.w = config.Ki_w ;
control_info_msg.Kd.x = config.Kd_x ;
control_info_msg.Kd.y = config.Kd_y ;
control_info_msg.Kd.z = config.Kd_z ;
control_info_msg.Kd.r = 0 ;
control_info_msg.Kd.p = 0 ;
control_info_msg.Kd.w = config.Kd_w ;
}
int main(int argc, char **argv)
{
// ROS node
ros::init(argc, argv, "move_quad");
ros::NodeHandle n;
ros::NodeHandle n_priv("~");
ros::Rate loop_rate(150);
// create an object
move_tube move_robot_obj(n);
while(ros::ok())
{
move_robot_obj.move(0, 0 , 0 );
sleep(2);
}
return 0;
}
int main(int argc, char **argv)
{
std::cout << "MAIN" << std::endl ;
/**
* The ros::init() function needs to see argc and argv so that it can perform
* any ROS arguments and name remapping that were provided at the command line. For programmatic
* remappings you can use a different version of init() which takes remappings
* directly, but for most command-line programs, passing argc and argv is the easiest
* way to do it. The third argument to init() is the name of the node.
*
* You must call one of the versions of ros::init() before using any other
* part of the ROS system.
*/
ros::init(argc, argv, "potential_field");
/**
* NodeHandle is the main access point to communications with the ROS system.
* The first NodeHandle constructed will fully initialize this node, and the last
* NodeHandle destructed will close down the node.
*/
ros::NodeHandle n;
ros::NodeHandle n_priv("~");
// parameters
double a_max;
double ro;
double freq;
double robot_mass;
double robot_radius;
double kp_x;
double kp_y;
double kp_z;
double kd_x;
double kd_y;
double kd_z;
// Control gains
n_priv.param<double>("Kp_x", kp_x, 1.0);
n_priv.param<double>("Kp_y", kp_y, 1.0);
n_priv.param<double>("Kp_z", kp_z, 1.0);
n_priv.param<double>("Kp_x", kd_x, 1.0);
n_priv.param<double>("Kp_y", kd_y, 1.0);
n_priv.param<double>("Kp_z", kd_z, 1.0);
Eigen::Vector3d kp(kp_x,kp_y,kp_z);
Eigen::Vector3d kd(kd_x,kd_y,kd_z);
double laser_max_distance;
//initialize operational parameters
n_priv.param<double>("laser_max_distance", laser_max_distance, 0.2);
n_priv.param<double>("ro", ro, 3.0);
n_priv.param<double>("frequency", freq, 10.0);
n_priv.param<double>("acc_max", a_max, 1.0);
n_priv.param<double>("robot_mass", robot_mass, 1.0);
n_priv.param<double>("robot_radius", robot_radius, 0.2);
ros::Rate loop_rate(freq);
std::string pose_topic_name = "/RosAria/pose" ;
std::string sonar_topic_name = "/RosAria/sonar";
ForceField potential_field(n, freq, ro, kp, kd, laser_max_distance, robot_mass, robot_radius, pose_topic_name, sonar_topic_name);
while(ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "uav_commander");
ros::NodeHandle n;
ros::NodeHandle n_priv("~");
std::string base_marker_id;
std::string head_marker_id;
double freq;
n_priv.param<double>("freq", freq, 50.0);
n_priv.param<std::string>("base_marker_id", base_marker_id, "teste");
n_priv.param<std::string>("head_marker_id", head_marker_id, "teste2");
///////////
// Gains //
///////////
// Proportional (kd)
double kp_x;
double kp_y;
double kp_z;
double kp_roll;
double kp_pitch;
double kp_yaw;
n_priv.param<double>("kp_x", kp_x, 0.5);
n_priv.param<double>("kp_y", kp_y, 1.0);
n_priv.param<double>("kp_z", kp_z, 1.0);
n_priv.param<double>("kp_roll", kp_roll, 0.0);
n_priv.param<double>("kp_pitch", kp_pitch, 0.0);
n_priv.param<double>("kp_yaw", kp_yaw, 1.0);
Eigen::Matrix<double,6,1> Kp;
Kp << kp_x,kp_y,kp_z, kp_roll, kp_pitch,kp_yaw;
// Derivative (kd)
double kd_x;
double kd_y;
double kd_z;
double kd_roll;
double kd_pitch;
double kd_yaw;
n_priv.param<double>("kd_x", kd_x, 1.0);
n_priv.param<double>("kd_y", kd_y, 1.0);
n_priv.param<double>("kd_z", kd_z, 1.0);
n_priv.param<double>("kd_roll", kd_roll, 0.0);
n_priv.param<double>("kd_pitch", kd_pitch, 0.0);
n_priv.param<double>("kd_yaw", kd_yaw, 1.0);
Eigen::Matrix<double,6,1> Kd;
Kd << kd_x,kd_y,kd_z, kd_roll, kd_pitch,kd_yaw;
std::cout << "Kp gains:" << Kp << std::endl;
std::cout << "Kd gains:" << Kd << std::endl;
PositionCommand position_commander(n,base_marker_id,head_marker_id, freq, Kp, Kd);
ros::Rate loop_rate(50);
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
return 0;
/*//ros::Publisher uav_commands = n.advertise<pctx_control::Control>("sendPCTXControl", 1000);
//ros::Subscriber sub = n.subscribe("TrackerPosition", 1000, getUpdatedPose);
ros::Rate loop_rate(15);
int count = 0;
std::vector<int16_t> controlValues(9,0);
double k = 3;
while (ros::ok())
{
pctx_control::Control controlMessage;
int val = count%1020;//int(sin((count%180)*PI/180.0)*1020);
//controlValues[0] = controlValues[1] =controlValues[2] =controlValues[3] =controlValues[4] =controlValues[5] =controlValues[6] =controlValues[7] =controlValues[8] = val;
controlValues[0] = k*y;
controlMessage.values = controlValues;
ROS_DEBUG("UAV_Commander broadcasting to all channels value:%d",controlValues[0]);
uav_commands.publish(controlMessage);
ros::spinOnce();
loop_rate.sleep();
count+=10;
}*/
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "controller");
ros::NodeHandle n;
ros::NodeHandle n_priv("~");
// parameters
double freq;
n_priv.param<double>("frequency", freq, 10.0);
double kp_x;
double kp_y;
double kp_z;
double kp_roll;
double kp_pitch;
double kp_yaw;
n_priv.param<double>("kp_x", kp_x, 1.0);
n_priv.param<double>("kp_y", kp_y, 1.0);
n_priv.param<double>("kp_z", kp_z, 1.0);
n_priv.param<double>("kp_roll", kp_roll, 1.0);
n_priv.param<double>("kp_pitch", kp_pitch, 1.0);
n_priv.param<double>("kp_yaw", kp_yaw, 1.0);
Eigen::Matrix<double,6,1> Kp;
Kp << kp_x,
kp_y,
kp_z,
kp_roll,
kp_pitch,
kp_yaw;
double kd_x;
double kd_y;
double kd_z;
double kd_roll;
double kd_pitch;
double kd_yaw;
n_priv.param<double>("kd_x", kd_x, 1.0);
n_priv.param<double>("kd_y", kd_y, 1.0);
n_priv.param<double>("kd_z", kd_z, 1.0);
n_priv.param<double>("kd_roll", kd_roll, 1.0);
n_priv.param<double>("kd_pitch", kd_pitch, 1.0);
n_priv.param<double>("kd_yaw", kd_yaw, 1.0);
Eigen::Matrix<double,6,1> Kd;
Kd << kd_x,
kd_y,
kd_z,
kd_roll,
kd_pitch,
kd_yaw;
double bd_x;
double bd_y;
double bd_z;
double bd_roll;
double bd_pitch;
double bd_yaw;
n_priv.param<double>("bd_x", bd_x, 1.0);
n_priv.param<double>("bd_y", bd_y, 1.0);
n_priv.param<double>("bd_z", bd_z, 1.0);
n_priv.param<double>("bd_roll", bd_roll, 1.0);
n_priv.param<double>("bd_pitch", bd_pitch, 1.0);
n_priv.param<double>("bd_yaw", bd_yaw, 1.0);
Eigen::Matrix<double,6,1> Bd;
Bd << bd_x,
bd_y,
bd_z,
bd_roll,
bd_pitch,
bd_yaw;
double lambda_x;
double lambda_y;
double lambda_z;
double lambda_roll;
double lambda_pitch;
double lambda_yaw;
n_priv.param<double>("lambda_x", lambda_x, 1.0);
n_priv.param<double>("lambda_y", lambda_y, 1.0);
n_priv.param<double>("lambda_z", lambda_z, 1.0);
n_priv.param<double>("lambda_roll", lambda_roll, 0.0);
n_priv.param<double>("lambda_pitch", lambda_pitch, 0.0);
n_priv.param<double>("lambda_yaw", lambda_yaw, 0.0);
Eigen::Matrix<double,6,6> lambda;
lambda << lambda_x, 0, 0, 0 ,0, 0,
0, lambda_y, 0, 0, 0, 0,
0, 0, lambda_z, 0, 0, 0,
0, 0, 0, lambda_roll, 0, 0,
0, 0, 0, 0, lambda_pitch, 0,
0, 0, 0, 0, 0, lambda_yaw;
double slave_min_x;
double slave_min_y;
double slave_min_z;
double slave_min_roll;
double slave_min_pitch;
double slave_min_yaw;
n_priv.param<double>("slave_min_x", slave_min_x, 1.0);
n_priv.param<double>("slave_min_y", slave_min_y, 1.0);
n_priv.param<double>("slave_min_z", slave_min_z, 1.0);
n_priv.param<double>("slave_min_roll", slave_min_roll, 1.0);
n_priv.param<double>("slave_min_pitch", slave_min_pitch, 1.0);
n_priv.param<double>("slave_min_yaw", slave_min_yaw, 1.0);
Eigen::Matrix<double,6,1> slave_min;
slave_min << slave_min_x,
slave_min_y,
slave_min_z,
slave_min_roll,
slave_min_pitch,
slave_min_yaw;
double slave_max_x;
double slave_max_y;
double slave_max_z;
double slave_max_roll;
double slave_max_pitch;
double slave_max_yaw;
n_priv.param<double>("slave_max_x", slave_max_x, 1.0);
n_priv.param<double>("slave_max_y", slave_max_y, 1.0);
n_priv.param<double>("slave_max_z", slave_max_z, 1.0);
n_priv.param<double>("slave_max_roll", slave_max_roll, 1.0);
n_priv.param<double>("slave_max_pitch", slave_max_pitch, 1.0);
n_priv.param<double>("slave_max_yaw", slave_max_yaw, 1.0);
Eigen::Matrix<double,6,1> slave_max;
slave_max << slave_max_x,
slave_max_y,
slave_max_z,
slave_max_roll,
slave_max_pitch,
slave_max_yaw;
Eigen::Matrix<double,6,1> slave_size=slave_max-slave_min;
double slave_velocity_min_x;
double slave_velocity_min_y;
double slave_velocity_min_z;
double slave_velocity_min_roll;
double slave_velocity_min_pitch;
double slave_velocity_min_yaw;
n_priv.param<double>("slave_velocity_min_x", slave_velocity_min_x, 1.0);
n_priv.param<double>("slave_velocity_min_y", slave_velocity_min_y, 1.0);
n_priv.param<double>("slave_velocity_min_z", slave_velocity_min_z, 1.0);
n_priv.param<double>("slave_velocity_min_roll", slave_velocity_min_roll, 1.0);
n_priv.param<double>("slave_velocity_min_pitch", slave_velocity_min_pitch, 1.0);
n_priv.param<double>("slave_velocity_min_yaw", slave_velocity_min_yaw, 1.0);
Eigen::Matrix<double,6,1> slave_velocity_min;
slave_velocity_min << slave_velocity_min_x,
slave_velocity_min_y,
slave_velocity_min_z,
slave_velocity_min_roll,
slave_velocity_min_pitch,
slave_velocity_min_yaw;
double slave_velocity_max_x;
double slave_velocity_max_y;
double slave_velocity_max_z;
double slave_velocity_max_roll;
double slave_velocity_max_pitch;
double slave_velocity_max_yaw;
n_priv.param<double>("slave_velocity_max_x", slave_velocity_max_x, 1.0);
n_priv.param<double>("slave_velocity_max_y", slave_velocity_max_y, 1.0);
n_priv.param<double>("slave_velocity_max_z", slave_velocity_max_z, 1.0);
n_priv.param<double>("slave_velocity_max_roll", slave_velocity_max_roll, 1.0);
n_priv.param<double>("slave_velocity_max_pitch", slave_velocity_max_pitch, 1.0);
n_priv.param<double>("slave_velocity_max_yaw", slave_velocity_max_yaw, 1.0);
Eigen::Matrix<double,6,1> slave_velocity_max;
slave_velocity_max << slave_velocity_max_x,
slave_velocity_max_y,
slave_velocity_max_z,
slave_velocity_max_roll,
slave_velocity_max_pitch,
slave_velocity_max_yaw;
Eigen::Matrix<double,6,1> slave_velocity_size=slave_velocity_max-slave_velocity_min;
double master_min_x;
double master_min_y;
double master_min_z;
double master_min_roll;
double master_min_pitch;
double master_min_yaw;
n_priv.param<double>("master_min_x", master_min_x, 1.0);
n_priv.param<double>("master_min_y", master_min_y, 1.0);
n_priv.param<double>("master_min_z", master_min_z, 1.0);
n_priv.param<double>("master_min_roll", master_min_roll, 1.0);
n_priv.param<double>("master_min_pitch", master_min_pitch, 1.0);
n_priv.param<double>("master_min_yaw", master_min_yaw, 1.0);
Eigen::Matrix<double,6,1> master_min;
master_min << master_min_x,
master_min_y,
master_min_z,
master_min_roll,
master_min_pitch,
master_min_yaw;
double master_max_x;
double master_max_y;
double master_max_z;
double master_max_roll;
double master_max_pitch;
double master_max_yaw;
n_priv.param<double>("master_max_x", master_max_x, 1.0);
n_priv.param<double>("master_max_y", master_max_y, 1.0);
n_priv.param<double>("master_max_z", master_max_z, 1.0);
n_priv.param<double>("master_max_roll", master_max_roll, 1.0);
n_priv.param<double>("master_max_pitch", master_max_pitch, 1.0);
n_priv.param<double>("master_max_yaw", master_max_yaw, 1.0);
Eigen::Matrix<double,6,1> master_max;
master_max << master_max_x,
master_max_y,
master_max_z,
master_max_roll,
master_max_pitch,
master_max_yaw;
Eigen::Matrix<double,6,1> master_size=master_max-master_min;
std::cout << "SLAVE VELOCITY SIZE:" << slave_velocity_size << std::endl;
bool is_master;
n_priv.param<bool>("is_master", is_master, true);
//Controller controller;
if(is_master)
{
Eigen::Matrix<double,6,1> slave_to_master_scale;
slave_to_master_scale << fabs(master_size(0,0)/slave_size(0,0)),
fabs(master_size(1,0)/slave_size(1,0)),
fabs(master_size(2,0)/slave_size(2,0)),
fabs(master_size(3,0)/slave_size(3,0)),
fabs(master_size(4,0)/slave_size(4,0)),
fabs(master_size(5,0)/slave_size(5,0));
Eigen::Matrix<double,6,1> slave_velocity_master_pose_scale;
slave_velocity_master_pose_scale << fabs(master_size(0,0)/slave_velocity_size(0,0)),
fabs(master_size(1,0)/slave_velocity_size(1,0)),
fabs(master_size(2,0)/slave_velocity_size(2,0)),
fabs(master_size(3,0)/slave_velocity_size(3,0)),
fabs(master_size(4,0)/slave_velocity_size(4,0)),
fabs(master_size(5,0)/slave_velocity_size(5,0));
MasterController controller(n, freq, Kp, Kd, Bd, lambda, slave_to_master_scale, slave_velocity_master_pose_scale, master_min, master_max, slave_min, slave_max, slave_velocity_min, slave_velocity_max);
ros::Rate loop_rate(freq);
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
}
else
{
Eigen::Matrix<double,6,1> master_to_slave_scale;
master_to_slave_scale << fabs(slave_size(0,0)/master_size(0,0)),
fabs(slave_size(1,0)/master_size(1,0)),
fabs(slave_size(2,0)/master_size(2,0)),
fabs(slave_size(3,0)/master_size(3,0)),
fabs(slave_size(4,0)/master_size(4,0)),
fabs(slave_size(5,0)/master_size(5,0));
Eigen::Matrix<double,6,1> master_pose_slave_velocity_scale;
master_pose_slave_velocity_scale << fabs(slave_velocity_size(0,0)/master_size(0,0)),
fabs(slave_velocity_size(1,0)/master_size(1,0)),
fabs(slave_velocity_size(2,0)/master_size(2,0)),
fabs(slave_velocity_size(3,0)/master_size(3,0)),
fabs(slave_velocity_size(4,0)/master_size(4,0)),
fabs(slave_velocity_size(5,0)/master_size(5,0));
std::cout << master_size(0,0) << " " << slave_velocity_size(0,0) << std::endl;
std::cout << "before:" <<master_pose_slave_velocity_scale << std::endl;
SlaveController controller(n, freq, Kp, Kd, Bd, lambda, master_to_slave_scale, master_pose_slave_velocity_scale, master_min, master_max, slave_min, slave_max, slave_velocity_min, slave_velocity_max);
ros::Rate loop_rate(freq);
while (ros::ok())
{
ros::spinOnce();
loop_rate.sleep();
}
}
return 0;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "uav_commander");
ros::NodeHandle n;
ros::NodeHandle n_priv("~");
////////////////////////////
// Load object part types //
////////////////////////////
std::vector<std::string> robot_ids;
std::vector<Eigen::Matrix<double,4,1, Eigen::DontAlign> > desired_formation;
XmlRpc::XmlRpcValue swarm;
n_priv.getParam("swarm", swarm);
ROS_INFO_STREAM("Loading swarm of " << swarm.size() << " robots.");
std::map<std::string, XmlRpc::XmlRpcValue>::iterator i;
for (i = swarm.begin(); i != swarm.end(); i++)
{
robot_ids.push_back(i->first);
Eigen::Matrix<double,4,1, Eigen::DontAlign> pose;
double x=i->second["pose"]["x"]["value"];
double y=i->second["pose"]["y"]["value"];
double z=i->second["pose"]["z"]["value"];
double yaw=i->second["pose"]["yaw"]["value"];
pose << x, y,z, yaw;
desired_formation.push_back(pose);
}
SwarmFormationControl swarm_formation(n,robot_ids,desired_formation);
ros::Publisher goal_pub=n.advertise<geometry_msgs::PoseStamped>("swarm_goal",10);
geometry_msgs::PoseStamped goal_msg;
goal_msg.pose.position.x=0.0;
goal_msg.pose.position.y=0.0;
goal_msg.pose.position.z=1.0;
// static tf::TransformBroadcaster brr;
// tf::Transform transform;
// int increment=1;
// tf::Quaternion q;
// q.setRPY(0, 0, 0);
// transform.setRotation(q);
// transform.setOrigin( tf::Vector3(0.1, 0.1, 1.0+0.01) );
ros::AsyncSpinner spinner(4);
spinner.start();
ros::Rate loop_rate(20);
while (ros::ok())
{
// ++increment;
// transform.setOrigin( tf::Vector3(1.0, 1.0, 1.0) );
// transform.setOrigin( tf::Vector3(0.1+increment*0.001, 0.1+increment*0.001, 1.0+increment*0.001) );
// q.setRPY(0, 0, 0+increment*0.001);
// transform.setRotation(q);
// brr.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "world", "swarm_goal"));
swarm_formation.updateCentroid();
loop_rate.sleep();
goal_msg.pose.position.x+=0.01;
goal_msg.pose.position.z+=0.001;
goal_pub.publish(goal_msg);
}
spinner.stop();
return 0;
}
