27 #ifndef _traj_executor_CPP_
28 #define _traj_executor_CPP_
42 #include <atlasmv_base/AtlasmvStatus.h>
43 #include <atlasmv_base/AtlasmvMotionCommand.h>
44 #include <pcl/conversions.h>
45 #include <pcl/point_cloud.h>
46 #include <pcl/point_types.h>
48 #include <boost/lexical_cast.hpp>
49 #include <boost/format.hpp>
50 #include <trajectory_planner/traj_info.h>
54 trajectory_planner::traj_info
info;
55 atlasmv_base::AtlasmvMotionCommand
command;
69 if(current_node != -1 && static_cast<int>(
info.alpha.size())>current_node)
75 std::cout<<
"DIR -> "<<
command.dir<<std::endl;
76 std::cout<<
"SPEED -> "<<
command.speed<<std::endl;
85 command.header.stamp=ros::Time::now();
90 gettimeofday(&tv, &tz);
91 tm=localtime(&tv.tv_sec);
92 printf(
"Tempooexec %d:%02d:%02d %d \n", tm->tm_hour, tm->tm_min,
93 tm->tm_sec, tv.tv_usec);
106 if(node!=-1 && static_cast<int>(
info.arc.size())>node)
158 int main(
int argc,
char **argv)
160 ros::init(argc, argv,
"trajectory_executor_nodelet");
161 ros::NodeHandle n(
"~");
165 ros::Subscriber subscribe_traj_info = n.subscribe (
"/trajectory_information", 1,
Info_handler);
166 ros::Subscriber subscribeStatusMessages = n.subscribe (
"/atlasmv/base/status", 1,
StatusMessageHandler);
167 ros::Rate loop_rate(10);
170 commandPublisher = n.advertise<atlasmv_base::AtlasmvMotionCommand>(
"/atlasmv/base/motion", 1);
178 if(
info.arc.size()==0)
187 if(jump_to_next_node)
189 if(node==static_cast<int>((
info.arc.size()-1)))
bool jump_node(double dist_init, int node)
Determines if the robot is already on the following node.
ros::Publisher commandPublisher
class which evaluates the trajectories planned
class which generates a few number of trajectories from an input file
trajectory_planner::traj_info info
void Info_handler(const trajectory_planner::traj_info &msg)
void send_command_message(int current_node)
Callback of published message.
int main(int argc, char **argv)
Main code of the nodelet.
atlasmv_base::AtlasmvMotionCommand command
void StatusMessageHandler(const atlasmv_base::AtlasmvStatus &msg)
Define message status.
atlasmv_base::AtlasmvStatus base_status