27 #ifndef _trajectory_planner_nodelet_H_
28 #define _trajectory_planner_nodelet_H_
33 #include <atlasmv_base/AtlasmvStatus.h>
34 #include <atlasmv_base/AtlasmvMotionCommand.h>
35 #include <pcl/conversions.h>
36 #include <pcl/point_cloud.h>
37 #include <pcl/point_types.h>
38 #include <mtt/TargetListPC.h>
40 #include <boost/lexical_cast.hpp>
41 #include <boost/format.hpp>
43 #include <tf/transform_broadcaster.h>
44 #include <tf/transform_listener.h>
45 #include <trajectory_planner/coordinates.h>
46 #include <geometry_msgs/Pose.h>
49 #define PFLN printf("FILE %s LINE %d\n",__FILE__, __LINE__);
54 #define _VEHICLE_HEIGHT_TOP_ 0.66
55 #define _VEHICLE_HEIGHT_BOTTOM_ 0.055
56 #define _VEHICLE_LENGHT_FRONT_ 0.62
57 #define _VEHICLE_LENGHT_BACK_ 0.2
58 #define _VEHICLE_WIDTH_ 0.42
61 using namespace visualization_msgs;
63 #ifdef _trajectory_planner_nodelet_CPP_
66 #define _EXTERN_ extern
75 int main(
int argc,
char **argv);
_EXTERN_ ros::NodeHandle * p_n
void send_command_message(vector< double > speed_setted, int current_node, c_trajectoryPtr t)
boost::shared_ptr< c_trajectory > c_trajectoryPtr
class which evaluates the trajectories planned
_EXTERN_ c_manage_trajectoryPtr manage_vt
_EXTERN_ tf::TransformListener * p_listener
class which generates a few number of trajectories from an input file
_EXTERN_ tf::StampedTransform transformw
boost::shared_ptr< c_manage_trajectory > c_manage_trajectoryPtr
_EXTERN_ ros::Publisher commandPublisher
vector< double > set_speed_vector(c_trajectoryPtr t)
Determine the speed vector.
void StatusMessageHandler(const atlasmv_base::AtlasmvStatus &msg)
Define message status.
bool jump_node(double dist_init, int node, c_trajectoryPtr t)
_EXTERN_ tf::StampedTransform transform_mtt
int main(int argc, char **argv)
Generates a frame higher than the car frame, to publish the point cloud to the mtt.