Skip to content

Commit

Permalink
Merge branch 'fix_init_poses'
Browse files Browse the repository at this point in the history
  • Loading branch information
@arturotorresg
arturotorresg committed Aug 1, 2017
2 parents 25fb857 + fbbfa7e commit e57068e
Show file tree
Hide file tree
Showing 4 changed files with 69 additions and 46 deletions.
35 changes: 16 additions & 19 deletions px4_bringup/config/simulation_params.yaml
View file
Original file line number Diff line number Diff line change
@@ -1,6 +1,3 @@
# Number of UAVs
number_of_uavs: 3

#--------------------------------------------------------------------------------------------#
# Gazebo origin (lat-lon-alt) can be defined with 'map_frame' or with 'sim_origin' params
#--------------------------------------------------------------------------------------------#
Expand All @@ -16,29 +13,29 @@ number_of_uavs: 3
sim_origin: [37.558542, -5.931074, 7.89]

#--------------------------------------------------------------------------------------------#
# uav frames
# uav initial positions
#--------------------------------------------------------------------------------------------#

# uav_1_home frame
uav_1_home_frame:
frame_id: "uav_1_home"
# uav_1_home
uav_1_home:
home_frame_id: "uav_1_home"
parent_frame: "map"
units: "m"
translation: [-10.0, -10.0, 0.3]
rotation: [0.0, 0.0, 0.0] # RPY
translation: [-10.0, -10.0, 0.0]
gz_initial_yaw: 0.0

# uav_2_home frame
uav_2_home_frame:
frame_id: "uav_2_home"
# uav_2_home
uav_2_home:
home_frame_id: "uav_2_home"
parent_frame: "map"
units: "m"
translation: [-10.0, 10.0, 0.3]
rotation: [0.0, 0.0, 0.0] # RPY
translation: [-10.0, 10.0, 0.0]
gz_initial_yaw: 0.0

# uav_3_home frame
uav_3_home_frame:
frame_id: "uav_3_home"
# uav_3_home
uav_3_home:
home_frame_id: "uav_3_home"
parent_frame: "map"
units: "m"
translation: [10.0, 0.0, 0.3]
rotation: [0.0, 0.0, 0.0] # RPY
translation: [10.0, 0.0, 0.0]
gz_initial_yaw: 0.0
18 changes: 12 additions & 6 deletions px4_bringup/scripts/spawn_gzmodel.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -114,11 +114,11 @@ def main():
tf_buffer = tf2_ros.Buffer(rospy.Duration(1200.0)) #tf buffer length
tf_listener = tf2_ros.TransformListener(tf_buffer)

if rospy.has_param( 'uav_{}_home_frame'.format(args.id) ):
uav_frame = rospy.get_param( 'uav_{}_home_frame'.format(args.id) )
if rospy.has_param( 'uav_{}_home'.format(args.id) ):
uav_frame = rospy.get_param( 'uav_{}_home'.format(args.id) )
if uav_frame['parent_frame']=='map':
robot_home = uav_frame['translation']
robot_yaw = uav_frame['rotation'][2]
robot_yaw = uav_frame['gz_initial_yaw']
elif uav_frame['parent_frame']=='game':
transform = tf_buffer.lookup_transform('map', 'game', rospy.Time(0), rospy.Duration(3.0))
pose_stamped = PoseStamped()
Expand All @@ -128,20 +128,26 @@ def main():
pose_stamped.pose.position.z = uav_frame['translation'][2]
pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform)
robot_home = [pose_transformed.pose.position.x,pose_transformed.pose.position.y, pose_transformed.pose.position.z]
robot_yaw = uav_frame['rotation'][2]
robot_yaw = uav_frame['gz_initial_yaw']
else:
robot_home = [0.0, 0.0, 0.0]
robot_yaw = 0.0
else:
robot_home = [0.0, 0.0, 0.0]
robot_yaw = 0.0


# Sleep for waiting the world to load
rospy.sleep(0.2)

# Minimum z to avoid collision with ground
z_min = 0.3

# Spawn robot sdf in gazebo
gzmodel_args = "gz model -f " + temp_sdf + \
" -m " + args.model + "_" + str(args.id) + \
" -x " + str(robot_home[0]) + \
" -y " + str(robot_home[1]) + \
" -z " + str(robot_home[2]) + \
" -z " + str(robot_home[2]+z_min) + \
" -Y " + str(robot_yaw)
subprocess.call(gzmodel_args, shell=True)

Expand Down
36 changes: 25 additions & 11 deletions uav_abstraction_layer/src/backend_light.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -345,23 +345,24 @@ bool BackendLight::referencePoseReached() const {
void BackendLight::initHomeFrame() {

uav_home_frame_id_ = "uav_" + std::to_string(robot_id_) + "_home";
local_start_pos_ << 0.0, 0.0, 0.0;

// Get frame from rosparam
std::string frame_id;
std::string parent_frame;
std::string units;
std::vector<double> translation;
std::vector<double> rotation;
double gz_yaw;
std::string uav_home_text;

uav_home_text = uav_home_frame_id_ + "_frame";
uav_home_text = uav_home_frame_id_;

if ( ros::param::has(uav_home_text) ) {
ros::param::get(uav_home_text + "/frame_id", frame_id);
ros::param::get(uav_home_text + "/home_frame_id", frame_id);
ros::param::get(uav_home_text + "/parent_frame", parent_frame);
ros::param::get(uav_home_text + "/units", units);
ros::param::get(uav_home_text + "/translation",translation);
ros::param::get(uav_home_text + "/rotation",rotation);
ros::param::get(uav_home_text + "/gz_initial_yaw",gz_yaw);

geometry_msgs::TransformStamped static_transformStamped;

Expand All @@ -371,12 +372,20 @@ void BackendLight::initHomeFrame() {
static_transformStamped.transform.translation.x = translation[0];
static_transformStamped.transform.translation.y = translation[1];
static_transformStamped.transform.translation.z = translation[2];
tf2::Quaternion quat;
quat.setRPY(rotation[0],rotation[1],rotation[2]);
static_transformStamped.transform.rotation.x = quat.x();
static_transformStamped.transform.rotation.y = quat.y();
static_transformStamped.transform.rotation.z = quat.z();
static_transformStamped.transform.rotation.w = quat.w();

if(parent_frame == "map" || parent_frame == "") {
static_transformStamped.transform.rotation.x = 0;
static_transformStamped.transform.rotation.y = 0;
static_transformStamped.transform.rotation.z = 0;
static_transformStamped.transform.rotation.w = 1;
}
else {
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
geometry_msgs::TransformStamped transform_to_map;
transform_to_map = tfBuffer.lookupTransform(parent_frame, "map", ros::Time(0), ros::Duration(2.0));
static_transformStamped.transform.rotation = transform_to_map.transform.rotation;
}

static_tf_broadcaster_ = new tf2_ros::StaticTransformBroadcaster();
static_tf_broadcaster_->sendTransform(static_transformStamped);
Expand All @@ -385,7 +394,12 @@ void BackendLight::initHomeFrame() {
cur_pose_.pose.position.x = 0.0;
cur_pose_.pose.position.y = 0.0;
cur_pose_.pose.position.z = 0.0;
cur_pose_.pose.orientation = static_transformStamped.transform.rotation;
tf2::Quaternion quat;
quat.setRPY(0.0,0.0,gz_yaw);
cur_pose_.pose.orientation.x = quat.x();
cur_pose_.pose.orientation.y = quat.y();
cur_pose_.pose.orientation.z = quat.z();
cur_pose_.pose.orientation.w = quat.w();
ref_pose_ = cur_pose_;
}
else {
Expand Down
26 changes: 16 additions & 10 deletions uav_abstraction_layer/src/backend_mavros.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -343,17 +343,15 @@ void BackendMavros::initHomeFrame() {
std::string parent_frame;
std::string units;
std::vector<double> translation;
std::vector<double> rotation;
std::string uav_home_text;

uav_home_text = uav_home_frame_id_ + "_frame";
uav_home_text = uav_home_frame_id_;

if ( ros::param::has(uav_home_text) ) {
ros::param::get(uav_home_text + "/frame_id", frame_id);
ros::param::get(uav_home_text + "/home_frame_id", frame_id);
ros::param::get(uav_home_text + "/parent_frame", parent_frame);
ros::param::get(uav_home_text + "/units", units);
ros::param::get(uav_home_text + "/translation",translation);
ros::param::get(uav_home_text + "/rotation",rotation);

geometry_msgs::TransformStamped static_transformStamped;

Expand All @@ -363,12 +361,20 @@ void BackendMavros::initHomeFrame() {
static_transformStamped.transform.translation.x = translation[0];
static_transformStamped.transform.translation.y = translation[1];
static_transformStamped.transform.translation.z = translation[2];
tf2::Quaternion quat;
quat.setRPY(rotation[0],rotation[1],rotation[2]);
static_transformStamped.transform.rotation.x = quat.x();
static_transformStamped.transform.rotation.y = quat.y();
static_transformStamped.transform.rotation.z = quat.z();
static_transformStamped.transform.rotation.w = quat.w();

if(parent_frame == "map" || parent_frame == "") {
static_transformStamped.transform.rotation.x = 0;
static_transformStamped.transform.rotation.y = 0;
static_transformStamped.transform.rotation.z = 0;
static_transformStamped.transform.rotation.w = 1;
}
else {
tf2_ros::Buffer tfBuffer;
tf2_ros::TransformListener tfListener(tfBuffer);
geometry_msgs::TransformStamped transform_to_map;
transform_to_map = tfBuffer.lookupTransform(parent_frame, "map", ros::Time(0), ros::Duration(2.0));
static_transformStamped.transform.rotation = transform_to_map.transform.rotation;
}

static_tf_broadcaster_ = new tf2_ros::StaticTransformBroadcaster();
static_tf_broadcaster_->sendTransform(static_transformStamped);
Expand Down

0 comments on commit e57068e

Please sign in to comment.