diff --git a/include/pauvsi_trajectory/Config.h b/include/pauvsi_trajectory/Config.h index 7ab4fe1..858067a 100644 --- a/include/pauvsi_trajectory/Config.h +++ b/include/pauvsi_trajectory/Config.h @@ -38,5 +38,7 @@ #define MAX_GEOM_REFINE_ITER 5 +#define PUBLISH_PATH true + #endif /* PAUVSI_TRAJECTORY_INCLUDE_PAUVSI_TRAJECTORY_CONFIG_H_ */ diff --git a/include/pauvsi_trajectory/Simulation.h b/include/pauvsi_trajectory/Simulation.h index c30feec..6fe11ee 100644 --- a/include/pauvsi_trajectory/Simulation.h +++ b/include/pauvsi_trajectory/Simulation.h @@ -9,7 +9,7 @@ #define PAUVSI_TRAJECTORY_INCLUDE_PAUVSI_TRAJECTORY_SIMULATION_H_ #define MOTOR_FORCE SIGMA 0.2 -#define POSITION_SIGMA 0.01 +#define POSITION_SIGMA 0.02 #define OMEGA_SIGMA 0.0005 #define VEL_SIGMA 0.01 #define QUAT_SIGMA 0.001 @@ -25,6 +25,9 @@ #define START_POS -9, -9, 0.5 +#define MOTOR_ABS_MAX 25 +#define MOTOR_ABS_MIN 0.1 + #endif /* PAUVSI_TRAJECTORY_INCLUDE_PAUVSI_TRAJECTORY_SIMULATION_H_ */ diff --git a/include/pauvsi_trajectory/TrajectoryGenerator.cpp b/include/pauvsi_trajectory/TrajectoryGenerator.cpp index c63ef61..ad3a37c 100644 --- a/include/pauvsi_trajectory/TrajectoryGenerator.cpp +++ b/include/pauvsi_trajectory/TrajectoryGenerator.cpp @@ -354,21 +354,21 @@ bool TrajectoryGenerator::testSegmentForGeometricFeasibility(TrajectorySegment s for(auto e : geoConstraints) { - if(e.type == e.PLANE_MIN) + if(e.type == e.Z_PLANE_MIN) { if(z < e.z_min) { failureTime = t; - ROS_DEBUG_STREAM("-failed at " << t); + ROS_DEBUG_STREAM("-failed at " << t << " z min: " << e.z_min); return false; } } - else if(e.type == e.PLANE_MAX) + else if(e.type == e.Z_PLANE_MAX) { if(z > e.z_max) { failureTime = t; - ROS_DEBUG_STREAM("+failed at " << t); + ROS_DEBUG_STREAM("+failed at " << t << " z min: " << e.z_max); return false; } } diff --git a/include/pauvsi_trajectory/Types.h b/include/pauvsi_trajectory/Types.h index cf81ec2..58d30c2 100644 --- a/include/pauvsi_trajectory/Types.h +++ b/include/pauvsi_trajectory/Types.h @@ -54,8 +54,8 @@ struct Point{ struct GeometricConstraint{ enum Type { - PLANE_MIN, - PLANE_MAX + Z_PLANE_MIN, + Z_PLANE_MAX }; Type type; @@ -71,11 +71,11 @@ struct GeometricConstraint{ type = _type; switch(type) { - case PLANE_MIN: - val = z_min; + case Z_PLANE_MIN: + z_min = val; break; - case PLANE_MAX: - val = z_max; + case Z_PLANE_MAX: + z_max = val; break; } } diff --git a/src/trajectory_server.cpp b/src/trajectory_server.cpp index 4ebd689..c070c1d 100644 --- a/src/trajectory_server.cpp +++ b/src/trajectory_server.cpp @@ -43,14 +43,18 @@ bool generateTrajectory(pauvsi_trajectory::trajectoryGeneration::Request &req, p TrajectoryGenerator tg; DynamicTrajectoryConstraints dc; + ROS_DEBUG_STREAM("request: " << req); + dc.start.pos = Point(req.startPosition.x, req.startPosition.y, req.startPosition.z); dc.start.vel = Point(req.startVelocity.x, req.startVelocity.y, req.startVelocity.z); dc.start.accel = Point(req.startAcceleration.x, req.startAcceleration.y, req.startAcceleration.z); dc.start.jerk = Point(req.startJerk.x, req.startJerk.y, req.startJerk.z); dc.start.snap = Point(req.startSnap.x, req.startSnap.y, req.startSnap.z); - dc.start.geoConstraint.push_back(GeometricConstraint(GeometricConstraint::PLANE_MAX, req.startMaxZ)); - dc.start.geoConstraint.push_back(GeometricConstraint(GeometricConstraint::PLANE_MIN, req.startMinZ)); + dc.start.geoConstraint.push_back(GeometricConstraint(GeometricConstraint::Z_PLANE_MIN, req.startMinZ)); + ROS_DEBUG_STREAM("start const 1 " << dc.start.geoConstraint.back().z_min); + dc.start.geoConstraint.push_back(GeometricConstraint(GeometricConstraint::Z_PLANE_MAX, req.startMaxZ)); + ROS_DEBUG_STREAM("start const 1 " << dc.start.geoConstraint.back().z_max); dc.end.pos = Point(req.goalPosition.x, req.goalPosition.y, req.goalPosition.z); dc.end.vel = Point(req.goalVelocity.x, req.goalVelocity.y, req.goalVelocity.z); @@ -58,11 +62,13 @@ bool generateTrajectory(pauvsi_trajectory::trajectoryGeneration::Request &req, p dc.end.jerk = Point(req.goalJerk.x, req.goalJerk.y, req.goalJerk.z); dc.end.snap = Point(req.goalSnap.x, req.goalSnap.y, req.goalSnap.z); - for(int i = 0; i < (int)req.middle_size; i++) + for(int i = 0; i < (int)req.middle.size(); i++) { dc.middle.push_back(BasicWaypointConstraint(Point(req.middle[i].x, req.middle[i].y, req.middle[i].z), 0)); - dc.middle[i].geoConstraint.push_back(GeometricConstraint(GeometricConstraint(GeometricConstraint::PLANE_MIN, req.middleGeometricConstraints[(2*i + 0)]))); - dc.middle[i].geoConstraint.push_back(GeometricConstraint(GeometricConstraint(GeometricConstraint::PLANE_MAX, req.middleGeometricConstraints[(2*i + 1)]))); + dc.middle[i].geoConstraint.push_back(GeometricConstraint(GeometricConstraint(GeometricConstraint::Z_PLANE_MIN, req.middleGeometricConstraints[(2*i + 0)]))); + ROS_DEBUG_STREAM("middle " << i << " min: " << dc.middle.at(i).geoConstraint.back().z_min); + dc.middle[i].geoConstraint.push_back(GeometricConstraint(GeometricConstraint(GeometricConstraint::Z_PLANE_MAX, req.middleGeometricConstraints[(2*i + 1)]))); + ROS_DEBUG_STREAM(" max " << dc.middle.at(i).geoConstraint.back().z_max); } //compute trajectory @@ -70,6 +76,10 @@ bool generateTrajectory(pauvsi_trajectory::trajectoryGeneration::Request &req, p res.tf = seg.tf; + res.trajectory.layout.dim.push_back(std_msgs::MultiArrayDimension()); + res.trajectory.layout.dim.push_back(std_msgs::MultiArrayDimension()); + res.trajectory.layout.dim.push_back(std_msgs::MultiArrayDimension()); + res.trajectory.layout.data_offset = 0; res.trajectory.layout.dim[0].label = "number of polynomials"; res.trajectory.layout.dim[0].stride = 3 * seg.x.size(); @@ -85,15 +95,25 @@ bool generateTrajectory(pauvsi_trajectory::trajectoryGeneration::Request &req, p for(int i = 0; i < (int)seg.x.size(); i++) { - res.trajectory.data.at(i) = seg.x[i]; + //res.trajectory.data.at(i) = seg.x[i]; + res.trajectory.data.push_back(seg.x[i]); } for(int i = 0; i < (int)seg.y.size(); i++) { - res.trajectory.data.at(seg.y.size()*1 + i) = seg.y[i]; + //res.trajectory.data.at(seg.y.size()*1 + i) = seg.y[i]; + res.trajectory.data.push_back(seg.y[i]); } for(int i = 0; i < (int)seg.z.size(); i++) { - res.trajectory.data.at(seg.z.size()*2 + i) = seg.z[i]; + //res.trajectory.data.at(seg.z.size()*2 + i) = seg.z[i]; + res.trajectory.data.push_back(seg.z[i]); + } + + if(PUBLISH_PATH) + { + nav_msgs::Path path = tg.generateTrajectorySegmentPath(seg); + path_pub.publish(path); + ROS_DEBUG_STREAM("published path"); } return true; diff --git a/src/virtual-flight-controller.cpp b/src/virtual-flight-controller.cpp index d564153..c0065e5 100644 --- a/src/virtual-flight-controller.cpp +++ b/src/virtual-flight-controller.cpp @@ -67,8 +67,8 @@ int main(int argc, char **argv) J << J_MATRIX; torque_transition << TORQUE_TRANSITION; mass = MASS; - min_motor_force = MOTOR_FORCE_MIN; - max_motor_force = MOTOR_FORCE_MAX; + min_motor_force = MOTOR_ABS_MIN; + max_motor_force = MOTOR_ABS_MAX; rng.seed(RANDOM_SEED); diff --git a/srv/trajectoryGeneration.srv b/srv/trajectoryGeneration.srv index ad7fff1..21d6ded 100644 --- a/srv/trajectoryGeneration.srv +++ b/srv/trajectoryGeneration.srv @@ -19,7 +19,6 @@ float64 startMaxZ # x2, y2, z2; # ... geometry_msgs/Vector3[] middle -int32 middle_size # minZ1, maxZ1; # minZ2, maxZ2; diff --git a/test/unit_test.cpp b/test/unit_test.cpp index af57035..47eff8d 100644 --- a/test/unit_test.cpp +++ b/test/unit_test.cpp @@ -169,11 +169,11 @@ int main(int argc, char **argv) GeometricConstraint floor, ceil, obs; - floor.type = GeometricConstraint::PLANE_MIN; + floor.type = GeometricConstraint::Z_PLANE_MIN; floor.z_min = 0; - obs.type = GeometricConstraint::PLANE_MIN; + obs.type = GeometricConstraint::Z_PLANE_MIN; obs.z_min = 2.25; - ceil.type = GeometricConstraint::PLANE_MAX; + ceil.type = GeometricConstraint::Z_PLANE_MAX; ceil.z_max = 3; dc.start.t = 0;