int main(int argc, char **argv)
{
ros::init(argc, argv, "dummy_startup");
ros::NodeHandle nh;
ros::NodeHandle p_nh("~");
client = nh.serviceClient<aero_srr_msgs::StateTransitionRequest>("/aero/supervisor/state_transition_request");
sub= nh.subscribe("aero/software_stop", 1, stopCB);
while(!got_free&&ros::ok())
{
ros::spinOnce();
}
return 1;
}
ArCalibration::ArCalibration(void)
{
ros::NodeHandle p_nh("~");
nh_.param<int>("marker_id", marker_id_, 0);
// initialize tag&arm position
tag_pos_.x = 0.0; tag_pos_.y = 0.0; tag_pos_.z = 0.0;
arm_pos_.position.x = 0.0; arm_pos_.position.y = 0.0; arm_pos_.position.z = 0.0;
arm_pos_.orientation.x = 0.0; arm_pos_.orientation.y = 0.0; arm_pos_.orientation.z = 0.0; arm_pos_.orientation.w = 1.0;
ar_marker_sub_ = nh_.subscribe("/ar_pose_marker", 1, &ArCalibration::arMarkerCallback, this);
arm_pos_sub_ = nh_.subscribe("/jaco_arm_driver/out/tool_position", 1, &ArCalibration::armPosCallback, this);
trigger_sub_ = nh_.subscribe("/joy", 1, &ArCalibration::joyCallback, this);
tag_pub_ = nh_.advertise<geometry_msgs::Point>("output_tag_position", 1);
arm_pub_ = nh_.advertise<geometry_msgs::Pose>("output_arm_position", 1);
ROS_INFO(" READY TO RECEIVE THE DATA FOR CALIBRATION");
}
int main(int argc, char ** argv)
{
//Initialize ROS
ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName);
// Allow 2 or 3 command line arguments
if (argc < 3 || argc > 4)
{
printf("Usage: tf_echo source_frame target_frame [echo_rate]\n\n");
printf("This will echo the transform from the coordinate frame of the source_frame\n");
printf("to the coordinate frame of the target_frame. \n");
printf("Note: This is the transform to get data from target_frame into the source_frame.\n");
printf("Default echo rate is 1 if echo_rate is not given.\n");
return -1;
}
ros::NodeHandle nh;
double rate_hz;
if (argc == 4)
{
// read rate from command line
rate_hz = atof(argv[3]);
}
else
{
// read rate parameter
ros::NodeHandle p_nh("~");
p_nh.param("rate", rate_hz, 1.0);
}
ros::Rate rate(rate_hz);
//Instantiate a local listener
echoListener echoListener;
std::string source_frameid = std::string(argv[1]);
std::string target_frameid = std::string(argv[2]);
// Wait for up to one second for the first transforms to become avaiable.
echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0));
//Nothing needs to be done except wait for a quit
//The callbacks withing the listener class
//will take care of everything
while(nh.ok())
{
try
{
tf::StampedTransform echo_transform;
echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform);
std::cout.precision(3);
std::cout.setf(std::ios::fixed,std::ios::floatfield);
std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl;
double yaw, pitch, roll;
echo_transform.getBasis().getRPY(roll, pitch, yaw);
tf::Quaternion q = echo_transform.getRotation();
tf::Vector3 v = echo_transform.getOrigin();
std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl;
std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", "
<< q.getZ() << ", " << q.getW() << "]" << std::endl
<< " in RPY (radian) [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl
<< " in RPY (degree) [" << roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl;
//print transform
}
catch(tf::TransformException& ex)
{
std::cout << "Failure at "<< ros::Time::now() << std::endl;
std::cout << "Exception thrown:" << ex.what()<< std::endl;
std::cout << "The current list of frames is:" <<std::endl;
std::cout << echoListener.tf.allFramesAsString()<<std::endl;
}
rate.sleep();
}
return 0;
};
int main (int argc, char **argv)
{
RT_TASK *task;
M3Sds * sys;
int cntr=0;
rt_allow_nonroot_hrt();
/*ros::init(argc, argv, "base_controller"); // initialize ROS node
ros::AsyncSpinner spinner(1); // Use 1 thread - check if you actually need this for only publishing
spinner.start();
ros::NodeHandle root_handle;*/
ros::init(argc, argv, "led_controller", ros::init_options::NoSigintHandler); // initialize ROS node
ros::AsyncSpinner spinner(1); // Use 1 thread - check if you actually need this for only publishing
spinner.start();
ros::NodeHandle root_handle;
ros::NodeHandle p_nh("~");
cmd_sub_g = root_handle.subscribe<shm_led_mouth::LEDMatrixCmd>("/led_matrix_command", 1, &commandCallback);
signal(SIGINT, endme);
if (sys = (M3Sds*)rt_shm_alloc(nam2num(MEKA_LED_SHM),sizeof(M3Sds),USE_VMALLOC))
printf("Found shared memory starting shm_led_mouth_controller.");
else
{
printf("Rtai_malloc failure for %s\n",MEKA_LED_SHM);
return 0;
}
rt_allow_nonroot_hrt();
/*if (!(task = rt_task_init_schmod(nam2num("TSHM"), RT_TASK_PRIORITY, 0, 0, SCHED_FIFO, 0xF)))
{
rt_shm_free(nam2num(TORQUE_SHM));
printf("Cannot init the RTAI task %s\n","TSHM");
return 0;
}*/
hst=rt_thread_create((void*)rt_system_thread, sys, 10000);
usleep(100000); //Let start up
if (!sys_thread_active)
{
rt_task_delete(task);
rt_shm_free(nam2num(MEKA_LED_SHM));
printf("Startup of thread failed.\n",0);
return 0;
}
while(!end)
{
usleep(250000);
}
printf("Removing RT thread...\n",0);
sys_thread_end=1;
//rt_thread_join(hst);
usleep(1250000);
if (sys_thread_active)printf("Real-time thread did not shutdown correctly\n");
//rt_task_delete(task);
rt_shm_free(nam2num(MEKA_LED_SHM));
ros::shutdown();
return 0;
}