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();
}
}
