int main(int argc, char **argv) { if (argc < 2) { ROS_ERROR("Usage: publish_database_object model_id"); exit(0); } //create handlers and initialize ros ros::Publisher marker_pub_; ros::ServiceClient get_model_mesh_srv_; ros::init(argc, argv, "publish_database_mesh"); ros::NodeHandle nh_(""); //advertise markers marker_pub_ = nh_.advertise<visualization_msgs::Marker>(nh_.resolveName("database_object"), 10); //wait a little but to make sure rviz subscribes ros::Duration(1.0).sleep(); //subscribe to service that gets a model mesh form the database std::string get_model_mesh_srv_name = "/objects_database_node/get_model_mesh"; while ( !ros::service::waitForService(get_model_mesh_srv_name, ros::Duration(2.0)) && nh_.ok() ) { ROS_INFO("Waiting for %s service to come up", get_model_mesh_srv_name.c_str()); } if (!nh_.ok()) exit(0); get_model_mesh_srv_ = nh_.serviceClient<household_objects_database_msgs::GetModelMesh> (get_model_mesh_srv_name, true); int model_id = atoi(argv[1]); ROS_INFO("Publishing mesh for model %d", model_id); //get the mesh household_objects_database_msgs::GetModelMesh get_mesh; get_mesh.request.model_id = model_id; if ( !get_model_mesh_srv_.call(get_mesh) || get_mesh.response.return_code.code != get_mesh.response.return_code.SUCCESS ) { ROS_ERROR("Failed to call database get mesh service for marker display"); exit(0); } visualization_msgs::Marker fitMarker = getFitMarker(get_mesh.response.mesh); //change here with the desired frame fitMarker.header.frame_id = "foo_frame"; fitMarker.header.stamp = ros::Time::now(); //publish at origin fitMarker.pose.orientation.w = 1; fitMarker.ns = "publish_database_object"; fitMarker.id = 0; marker_pub_.publish(fitMarker); ROS_INFO("Marker published"); //wait a little but to make sure the marker gets to rviz ros::Duration(1.0).sleep(); }
/** * @brief main * @param argc * @param argv * @return */ int main (int argc, char** argv) { //ROS Initialization ros::init(argc, argv, _nodeName); ROS_INFO("%s connected to roscore",_nodeName.c_str()); ros::NodeHandle nh_("~");//ROS node Handler absolute & relative path ros::Rate rate(_freq); while (ros::ok()) { ros::spinOnce(); rate.sleep(); } ROS_INFO("@%s Node terminated\n",_nodeName.c_str()); }
int main( int argc, char** argv ) { const int startup_delay = 5; ros::init(argc, argv, "rx60_hardware"); ros::NodeHandle nh_("robot_rx60b"); RX60Robot robot; RX60_wrapper wrapper; controller_manager::ControllerManager cm(&robot, nh_); ros::Publisher state_publisher = nh_.advertise<sensor_msgs::JointState>("/robot_rx60b/controller_joint_states", 10); ros::AsyncSpinner spinner(0); spinner.start(); //ros::MultiThreadedSpinner spinner(4); //spinner.spin(); ros::Time last = ros::Time::now(); ros::Rate r(10); int alive_count = 0; ros::Time startup_time = ros::Time::now(); while (ros::ok()) { ros::Duration period = ros::Time::now() - last; const sensor_msgs::JointState::Ptr robotState = wrapper.getJointState(); robot.read(robotState); // Wait for valid jointstates const ros::Duration time_since_start = ros::Time::now() - startup_time; if (time_since_start.toSec() > startup_delay) { cm.update(ros::Time::now(), period); const sensor_msgs::JointState::Ptr robotCmd = robot.write(); wrapper.setJointState(robotCmd); } else ROS_INFO("Waiting %i seconds before controlling the robot (%f elapsed)", startup_delay, time_since_start.toSec()); last = ros::Time::now(); state_publisher.publish(robotState); ros::spinOnce(); r.sleep(); } }