00001 #include <motion_planning_msgs/DisplayTrajectory.h> 00002 #include <planning_environment/monitors/joint_state_monitor.h> 00003 #include <boost/thread.hpp> 00004 00005 void spinThread() 00006 { 00007 ros::spin(); 00008 } 00009 00010 int main(int argc, char** argv) 00011 { 00012 ros::init(argc, argv, "display_trajectory_publisher"); 00013 boost::thread spin_thread(&spinThread); 00014 ros::NodeHandle root_handle; 00015 planning_environment::JointStateMonitor joint_state_monitor; 00016 ros::Publisher display_trajectory_publisher = root_handle.advertise<motion_planning_msgs::DisplayTrajectory>("joint_path_display", 1); 00017 while(display_trajectory_publisher.getNumSubscribers() < 1 && root_handle.ok()) 00018 { 00019 ROS_INFO("Waiting for subscriber"); 00020 ros::Duration(0.1).sleep(); 00021 } 00022 motion_planning_msgs::DisplayTrajectory display_trajectory; 00023 unsigned int num_points = 100; 00024 00025 display_trajectory.model_id = "pr2"; 00026 display_trajectory.trajectory.joint_trajectory.header.frame_id = "base_footprint"; 00027 display_trajectory.trajectory.joint_trajectory.header.stamp = ros::Time::now(); 00028 display_trajectory.trajectory.joint_trajectory.joint_names.push_back("r_shoulder_pan_joint"); 00029 display_trajectory.trajectory.joint_trajectory.points.resize(num_points); 00030 00031 for(unsigned int i=0; i < num_points; i++) 00032 { 00033 display_trajectory.trajectory.joint_trajectory.points[i].positions.push_back(-(M_PI*i/4.0)/num_points); 00034 } 00035 display_trajectory.robot_state.joint_state = joint_state_monitor.getJointStateRealJoints(); 00036 ROS_INFO("Publishing path for display"); 00037 display_trajectory_publisher.publish(display_trajectory); 00038 joint_state_monitor.stop(); 00039 ros::shutdown(); 00040 spin_thread.join(); 00041 return(0); 00042 }