int open_iob(void)
{
static bool isInitialized = false;
if ( isInitialized ) return TRUE;
isInitialized = true;
std::cerr << "[iob] Open IOB / start " << std::endl;
std::string node_name;
{
char *ret = getenv("HRPSYS_GAZEBO_IOB_NAME");
if (ret != NULL) {
node_name.assign(ret);
} else {
node_name = "hrpsys_gazebo_iob";
}
std::cerr << "[iob] set node name : " << node_name << std::endl;
}
std::map<std::string, std::string> arg;
ros::init(arg, "hrpsys_gazebo_iob", ros::init_options::NoSigintHandler);
rosnode = new ros::NodeHandle();
ros::WallDuration(0.5).sleep(); // wait for initializing ros
std::string controller_name;
{ // setting configuration name
char *ret = getenv("HRPSYS_GAZEBO_CONFIGURATION");
if (ret != NULL) {
controller_name.assign(ret);
} else {
controller_name = "hrpsys_gazebo_configuration";
}
ROS_INFO_STREAM( "[iob] set controller_name : " << controller_name);
}
std::string robotname;
{ // setting configuration name
char *ret = getenv("HRPSYS_GAZEBO_ROBOTNAME");
if (ret != NULL) {
robotname.assign(ret);
// controller_name -> robotname/controller_name
controller_name = robotname + "/" + controller_name;
} else {
std::string rname_str = std::string(controller_name) + "/robotname";
rosnode->getParam(rname_str, robotname);
}
if (robotname.empty()) {
ROS_ERROR("[iob] did not find robot_name");
robotname = "default";
}
ROS_INFO_STREAM( "[iob] set robot_name : " << robotname);
}
{ // setting synchronized
char *ret = getenv("HRPSYS_GAZEBO_IOB_SYNCHRONIZED");
if (ret != NULL) {
std::string ret_str(ret);
if (ret_str.size() > 0) {
iob_synchronized = true;
}
} else {
iob_synchronized = false;
}
if(rosnode->hasParam(controller_name + "/use_synchronized_command")) {
rosnode->getParam(controller_name + "/use_synchronized_command", iob_synchronized);
}
if(iob_synchronized) ROS_INFO("[iob] use synchronized command");
}
{ // setting substeps
char *ret = getenv("HRPSYS_GAZEBO_IOB_SUBSTEPS");
if (ret != NULL) {
int num = atoi(ret);
if (num > 0) {
num_of_substeps = num;
ROS_INFO("[iob] use substeps %d", num);
}
}
if(rosnode->hasParam(controller_name + "/iob_substeps")) {
rosnode->getParam(controller_name + "/iob_substeps", num_of_substeps);
ROS_INFO("[iob] use substeps %d", num_of_substeps);
}
}
{ // settting rate
if(rosnode->hasParam(controller_name + "/iob_rate")) {
double rate = 0;
rosnode->getParam(controller_name + "/iob_rate", rate);
overwrite_g_period_ns = (long) ((1000 * 1000 * 1000) / rate);
fprintf(stderr, "iob::period %d\n", overwrite_g_period_ns);
ROS_INFO("[iob] period_ns %d", overwrite_g_period_ns);
}
}
joint_real2model_vec.resize(0);
if (rosnode->hasParam(controller_name + "/joint_id_list")) {
XmlRpc::XmlRpcValue param_val;
rosnode->getParam(controller_name + "/joint_id_list", param_val);
if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for(int i = 0; i < param_val.size(); i++) {
int num = param_val[i];
joint_real2model_vec.push_back(num);
}
} else {
ROS_WARN("[iob] %s/joint_id_list is not list of integer", controller_name.c_str());
}
} else {
ROS_DEBUG("[iob] %s/joint_id_list is nothing", controller_name.c_str());
}
if (rosnode->hasParam(controller_name + "/use_velocity_feedback")) {
bool ret;
rosnode->getParam(controller_name + "/use_velocity_feedback", ret);
use_velocity_feedback = ret;
if(ret) {
ROS_INFO("[iob] use_velocity_feedback");
}
}
XmlRpc::XmlRpcValue param_val;
std::vector<std::string> joint_lst;
rosnode->getParam(controller_name + "/joints", param_val);
if (param_val.getType() == XmlRpc::XmlRpcValue::TypeArray) {
for(unsigned int s = 0; s < param_val.size(); s++) {
std::string nstr = param_val[s];
ROS_DEBUG("add joint: %s", nstr.c_str());
joint_lst.push_back(nstr);
}
} else {
ROS_ERROR("[iob] %s/joints is not list of joint name", controller_name.c_str());
}
if (joint_real2model_vec.size() == 0) {
for(unsigned int i = 0; i < joint_lst.size(); i++) {
joint_real2model_vec.push_back(i);
}
} else if (joint_real2model_vec.size() != joint_lst.size()) {
ROS_ERROR("[iob] size differece on joint_id_list and joints (%ld, %ld)",
joint_real2model_vec.size(), joint_lst.size());
}
for(unsigned int i = 0; i < joint_real2model_vec.size(); i++) {
joint_model2real_map[joint_real2model_vec[i]] = i;
}
unsigned int n = NUM_OF_REAL_JOINT;
initial_jointcommand.position.resize(n);
initial_jointcommand.velocity.resize(n);
//initial_jointcommand.effort.resize(n);
initial_jointcommand.effort.resize(0);
if(!use_velocity_feedback) {
initial_jointcommand.kp_position.resize(n);
initial_jointcommand.ki_position.resize(n);
initial_jointcommand.kd_position.resize(n);
initial_jointcommand.kp_velocity.resize(n);
initial_jointcommand.i_effort_min.resize(n);
initial_jointcommand.i_effort_max.resize(n);
} else {
initial_jointcommand.kpv_position.resize(n);
initial_jointcommand.kpv_velocity.resize(n);
}
for (unsigned int i = 0; i < joint_lst.size(); ++i) {
std::string joint_ns(controller_name);
joint_ns += ("/gains/" + joint_lst[i] + "/");
if (!use_velocity_feedback) {
double p_val = 0, i_val = 0, d_val = 0, i_clamp_val = 0, vp_val = 0;
std::string p_str = std::string(joint_ns)+"p";
std::string i_str = std::string(joint_ns)+"i";
std::string d_str = std::string(joint_ns)+"d";
std::string i_clamp_str = std::string(joint_ns)+"i_clamp";
std::string vp_str = std::string(joint_ns)+"vp";
if (!rosnode->getParam(p_str, p_val)) {
ROS_WARN("[iob] couldn't find a P param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(i_str, i_val)) {
ROS_WARN("[iob] couldn't find a I param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(d_str, d_val)) {
ROS_WARN("[iob] couldn't find a D param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(i_clamp_str, i_clamp_val)) {
ROS_WARN("[iob] couldn't find a I_CLAMP param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(vp_str, vp_val)) {
ROS_WARN("[iob] couldn't find a VP param for %s", joint_ns.c_str());
}
// store these directly on altasState, more efficient for pub later
initial_jointcommand.kp_position[i] = p_val;
initial_jointcommand.ki_position[i] = i_val;
initial_jointcommand.kd_position[i] = d_val;
initial_jointcommand.i_effort_min[i] = -i_clamp_val;
initial_jointcommand.i_effort_max[i] = i_clamp_val;
initial_jointcommand.velocity[i] = 0;
//initial_jointcommand.effort[i] = 0;
initial_jointcommand.kp_velocity[i] = vp_val;
} else {
// velocity feedback
double p_v_val = 0, vp_v_val = 0;
std::string p_v_str = std::string(joint_ns) + "p_v";
std::string vp_v_str = std::string(joint_ns) + "vp_v";
if (!rosnode->getParam(p_v_str, p_v_val)) {
ROS_WARN("[iob] couldn't find a P_V param for %s", joint_ns.c_str());
}
if (!rosnode->getParam(vp_v_str, vp_v_val)) {
ROS_WARN("[iob] couldn't find a VP_V param for %s", joint_ns.c_str());
}
initial_jointcommand.kpv_position[i] = p_v_val;
initial_jointcommand.kpv_velocity[i] = vp_v_val;
}
}
initial_jointcommand.desired_controller_period_ms =
static_cast<unsigned int>(overwrite_g_period_ns * 1e-6);
if (iob_synchronized) {
serv_command =
ros::service::createClient<hrpsys_gazebo_msgs::SyncCommand> (robotname + "/iob_command", true); // persistent = true,
ROS_INFO_STREAM("[iob] waiting service " << robotname << "/iob_command");
serv_command.waitForExistence();
ROS_INFO_STREAM("[iob] found service " << robotname << "/iob_command");
} else {
pub_joint_command = rosnode->advertise <JointCommand> (robotname + "/joint_command", 1, true);
// ros topic subscribtions
ros::SubscribeOptions jointStatesSo =
ros::SubscribeOptions::create<RobotState>(robotname + "/robot_state", 1, setJointStates,
ros::VoidPtr(), rosnode->getCallbackQueue());
// Because TCP causes bursty communication with high jitter,
// declare a preference on UDP connections for receiving
// joint states, which we want to get at a high rate.
// Note that we'll still accept TCP connections for this topic
// (e.g., from rospy nodes, which don't support UDP);
// we just prefer UDP.
// temporary use TCP / Using UDP occured some problem when message size more than 1500.
//jointStatesSo.transport_hints =
//ros::TransportHints().maxDatagramSize(3000).unreliable().reliable().tcpNoDelay(true);
jointStatesSo.transport_hints =
ros::TransportHints().reliable().tcpNoDelay(true);
sub_robot_state = rosnode->subscribe(jointStatesSo);
}
for (int i=0; i < number_of_joints(); i++){
command[i] = 0.0;
power[i] = OFF;
servo[i] = OFF;
}
clock_gettime(CLOCK_MONOTONIC, &g_ts);
rg_ts = ros::Time::now();
jointcommand = initial_jointcommand;
std::cerr << "[iob] " << number_of_joints() << " / " << initial_jointcommand.position.size() << " / " << NUM_OF_REAL_JOINT << std::endl;
if (iob_synchronized) {
hrpsys_gazebo_msgs::SyncCommandRequest req;
req.joint_command = jointcommand;
hrpsys_gazebo_msgs::SyncCommandResponse res;
std::cerr << "[iob] first service call" << std::endl;
serv_command.call(req, res);
std::cerr << "[iob] first service returned" << std::endl;
js = res.robot_state;
init_sub_flag = true;
} else {
std::cerr << "[iob] block until subscribing first robot_state";
ros::Time start_tm = ros::Time::now();
ros::Rate rr(100);
ros::spinOnce();
while (!init_sub_flag) {
if ((ros::Time::now() - start_tm).toSec() > 5.0) {
std::cerr << "[iob] timeout for waiting robot_state";
break;
}
std::cerr << ".";
rr.sleep();
ros::spinOnce();
}
}
std::cerr << std::endl << "[iob] Open IOB / finish " << std::endl;
return TRUE;
}
bool testDummyAction(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res)
{
ROS_INFO("Beginning dummy action test");
dummy_action(5);
return true;
}
int pcd_from_ply_main(int argc, char **argv)
{
if (pcl::console::find_switch (argc, argv, "-h"))
{
show_help (argv[0]);
exit (0);
}
// parse the command line
std::vector<int> ply_idx = pcl::console::parse_file_extension_argument (argc, argv, ".ply");
std::string ply_file_path( argv[ply_idx[0]] );
std::vector<int> pcd_idx = pcl::console::parse_file_extension_argument (argc, argv, ".pcd");
std::string pcd_file_path( argv[pcd_idx[0]] );
// default options
double vpx = 1.0, vpy = 0.0, vpz = 0.0;
double tpx = 0.0, tpy = 0.0, tpz = 0.0;
int coord = 1;
bool org = false, add_noise = false, ascii = false;
double sub_leaf_size = -1.0;
// command line options
pcl::console::parse_3x_arguments (argc, argv, "-vp", vpx, vpy, vpz);
pcl::console::parse_3x_arguments (argc, argv, "-tp", tpx, tpy, tpz);
pcl::console::parse_argument (argc, argv, "-coord", coord);
pcl::console::parse_argument (argc, argv, "-add_noise", add_noise);
pcl::console::parse_argument (argc, argv, "-org", org);
pcl::console::parse_argument (argc, argv, "-ascii", ascii);
pcl::console::parse_argument (argc, argv, "-subsamp", sub_leaf_size);
// Construct and initialize the virtual kinect
vkin_offline vko( Eigen::Vector3d(0,0,0), Eigen::Vector4d(0,0,0,1), coord, org, add_noise );
vko.init_vkin( ply_file_path );
// get a noiseless cloud
pcl::PointCloud<pcl::PointXYZ>::Ptr cld_ptr( vko.sense( Eigen::Vector3d(vpx, vpy, vpz),
Eigen::Vector3d(tpx, tpy, tpz) ) );
if( sub_leaf_size > 0 )
cld_ptr = pcd_utils::voxel_grid_subsample( cld_ptr, static_cast<float>(sub_leaf_size) );
// save the scan to file
pcl::PCDWriter writer;
if(ascii)
writer.writeASCII( pcd_file_path.c_str(), *cld_ptr );
else
writer.writeBinaryCompressed( pcd_file_path.c_str(), *cld_ptr );
// Extract the tree scores if requested
std::string score_save_path;
if (pcl::console::parse_argument (argc, argv, "-save_scores", score_save_path) != -1)
{
// Create and initialize an nbv tree
std::string vision_module_path(ros::package::getPath("vision_module"));
vtree_user vt("/load", vision_module_path + "/data", vision_module_path + "/../data/cloud_data");
vt.start();
/*
ros::init(argc, argv, "ply2pcd_node");
ros::NodeHandle node_handle("~");
NBVTree nbv_tree(node_handle);
nbv_tree.start();
*/
// Get the score list
ROS_INFO("Getting matches...");
std::vector<std::pair<float,std::string> > match_names;
vt.match_list( cld_ptr->getMatrixXfMap(), match_names, 100 );
// write to file
std::ofstream outstr;
io_utils::open_out_file( outstr, score_save_path );
if( !outstr )
throw std::runtime_error("Cannot open scores save file...\n");
for( size_t i = 0; i < match_names.size(); ++i)
{
outstr << match_names[i].first << " " << match_names[i].second << std::endl;
//outstr << cluster_match_names[i].first << " " << cluster_match_names[i].second << std::endl;
}
outstr.close();
}
return 0;
}
void help()
{
ROS_INFO("#####################################################");
ROS_INFO("RVIZ SETUP");
ROS_INFO("----------");
ROS_INFO(" Global options:");
ROS_INFO(" FixedFrame = /base_footprint");
ROS_INFO(" Add a RobotState display:");
ROS_INFO(" RobotDescription = robot_description");
ROS_INFO(" RobotStateTopic = interactive_robot_state");
ROS_INFO(" Add a Marker display:");
ROS_INFO(" MarkerTopic = interactive_robot_markers");
ROS_INFO(" Add an InteractiveMarker display:");
ROS_INFO(" UpdateTopic = interactive_robot_imarkers/update");
ROS_INFO(" Add a MarkerArray display:");
ROS_INFO(" MarkerTopic = interactive_robot_marray");
ROS_INFO("#####################################################");
}
//test get the current time
bool testGetTime(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
ROS_INFO("Getting the current local time");
std::string t = getCurrentStringTime();
ROS_INFO("current time: %s, seconds since midnight: %ld",t.c_str(),secondsFromStringTime(t));
return true;
}
void Square::sensorCallback(const create_fundamentals::SensorPacket::ConstPtr& msg){
ROS_INFO("left encoder: %f, right encoder: %f", msg->encoderLeft, msg->encoderRight);
}
DvsRosDriver::DvsRosDriver(ros::NodeHandle & nh, ros::NodeHandle nh_private) :
nh_(nh), parameter_update_required_(false)
{
// load parameters
nh_private.param<std::string>("serial_number", device_id_, "");
nh_private.param<bool>("master", master_, true);
double reset_timestamps_delay;
nh_private.param<double>("reset_timestamps_delay", reset_timestamps_delay, -1.0);
// start driver
bool device_is_running = false;
while (!device_is_running)
{
const char* serial_number_restrict = (device_id_ == "") ? NULL : device_id_.c_str();
dvs128_handle = caerDeviceOpen(1, CAER_DEVICE_DVS128, 0, 0, serial_number_restrict);
//dvs_running = driver_->isDeviceRunning();
device_is_running = !(dvs128_handle == NULL);
if (!device_is_running)
{
ROS_WARN("Could not find DVS. Will retry every second.");
ros::Duration(1.0).sleep();
ros::spinOnce();
}
else
{
// configure as master or slave
caerDeviceConfigSet(dvs128_handle, DVS128_CONFIG_DVS, DVS128_CONFIG_DVS_TS_MASTER, master_);
}
if (!ros::ok())
{
return;
}
}
dvs128_info_ = caerDVS128InfoGet(dvs128_handle);
device_id_ = "DVS128-V1-" + std::string(dvs128_info_.deviceString).substr(15, 4);
ROS_INFO("%s --- ID: %d, Master: %d, DVS X: %d, DVS Y: %d, Logic: %d.\n", dvs128_info_.deviceString,
dvs128_info_.deviceID, dvs128_info_.deviceIsMaster, dvs128_info_.dvsSizeX, dvs128_info_.dvsSizeY,
dvs128_info_.logicVersion);
current_config_.streaming_rate = 30;
delta_ = boost::posix_time::microseconds(1e6/current_config_.streaming_rate);
// set namespace
std::string ns = ros::this_node::getNamespace();
if (ns == "/")
ns = "/dvs";
event_array_pub_ = nh_.advertise<dvs_msgs::EventArray>(ns + "/events", 1);
camera_info_pub_ = nh_.advertise<sensor_msgs::CameraInfo>(ns + "/camera_info", 1);
// camera info handling
ros::NodeHandle nh_ns(ns);
camera_info_manager_ = new camera_info_manager::CameraInfoManager(nh_ns, device_id_);
// reset timestamps is publisher as master, subscriber as slave
if (master_)
{
reset_pub_ = nh_.advertise<std_msgs::Time>((ns + "/reset_timestamps").c_str(), 1);
}
else
{
reset_sub_ = nh_.subscribe((ns + "/reset_timestamps").c_str(), 1, &DvsRosDriver::resetTimestampsCallback, this);
}
// initialize timestamps
resetTimestamps();
// spawn threads
running_ = true;
parameter_thread_ = boost::shared_ptr< boost::thread >(new boost::thread(boost::bind(&DvsRosDriver::changeDvsParameters, this)));
readout_thread_ = boost::shared_ptr< boost::thread >(new boost::thread(boost::bind(&DvsRosDriver::readout, this)));
// Dynamic reconfigure
dynamic_reconfigure_callback_ = boost::bind(&DvsRosDriver::callback, this, _1, _2);
server_.reset(new dynamic_reconfigure::Server<dvs_ros_driver::DVS_ROS_DriverConfig>(nh_private));
server_->setCallback(dynamic_reconfigure_callback_);
// start timer to reset timestamps for synchronization
if (reset_timestamps_delay > 0.0 && master_)
{
timestamp_reset_timer_ = nh_.createTimer(ros::Duration(reset_timestamps_delay), &DvsRosDriver::resetTimerCallback, this);
ROS_INFO("Started timer to reset timestamps on master DVS for synchronization (delay=%3.2fs).", reset_timestamps_delay);
}
}
BumperToMTSA(ros::NodeHandle node_handle,ros::NodeHandle private_node_handle )
: nh(node_handle), private_nh(private_node_handle)
{
ROS_INFO("BumperToMTSA");
pickput_detect = 0;
}
// Main code:
int main(int argc,char* argv[])
{
// (when V-REP launches this executable, V-REP will also provide the argument list)
//numero de argumentos que mande (se excluye el fantasma que el manda solo)
int Narg=0, handleArana=0, k=0;
int PataTipHandle[Npatas],FuerzaSensorHandle[Npatas],Pata_Motor1Handle[Npatas];
float periodo, f;
camina::UbicacionRobot ubicacionRobot;
geometry_msgs::PoseStamped CuerpoPose;
geometry_msgs::PoseStamped PataTipPose[Npatas];
tf::Quaternion CuerpoOrientacion_Q;
tfScalar roll, pitch, yaw;
Narg=13;
if (argc>=Narg)
{
handleArana=atoi(argv[1]);
for (k=0;k<Npatas;k++) PataTipHandle[k] = atoi(argv[2+k]);
for (k=0;k<Npatas;k++) FuerzaSensorHandle[k] = atoi(argv[2+Npatas+k]);
for (k=0;k<Npatas;k++) Pata_Motor1Handle[k] = atoi(argv[2+2*Npatas+k]);
}
else
{
ROS_ERROR("Nodo6:Indique argumentos completos!\n");
return (0);
}
// Inicializacion de variables del mensaje
for (k=0;k<Npatas;k++) {
ubicacionRobot.coordenadaPata_y.push_back(0);
ubicacionRobot.coordenadaPata_x.push_back(0);
ubicacionRobot.coordenadaPata_z.push_back(0);
ubicacionRobot.coordenadaPataSistemaPata_y.push_back(0);
ubicacionRobot.coordenadaPataSistemaPata_x.push_back(0);
ubicacionRobot.coordenadaPataSistemaPata_z.push_back(0);
ubicacionRobot.pataTipFuerza_z.push_back(0);
ubicacionRobot.pataApoyo.push_back(0);
}
// Create a ROS node:
int _argc = 0;
char** _argv = NULL;
ros::init(_argc,_argv,"Nodo6_UbicacionArana");
if(!ros::master::check()) return(0);
ros::NodeHandle node;
ROS_INFO("Nodo6_UbicacionArana just started\n");
periodo=0.1;
f=1/periodo;
ros::Rate loop_rate(f); //Frecuencia [Hz]
//Topicos susbcritos y publicados
ros::Subscriber subInfo=node.subscribe("/vrep/info",1,infoCallback);
ros::Publisher chatter_pub = node.advertise<camina::UbicacionRobot>("UbicacionRobot", 100);
//Clientes y Servicios
client_simRosGetObjectPose=node.serviceClient<vrep_common::simRosGetObjectPose>("/vrep/simRosGetObjectPose");
client_simRosReadForceSensor=node.serviceClient<vrep_common::simRosReadForceSensor>("/vrep/simRosReadForceSensor");
srv_simRosGetObjectPose.request.relativeToObjectHandle=-1;
// Creamos archivo para guardar posiciones de patas
// fp = fopen("../fuerte_workspace/sandbox/TesisMaureen/ROS/camina/datos/XYPatas.txt","w+");
// int cuentaPataApoyo=0, pataApoyo[Npatas];
while (ros::ok() && simulationRunning)
{
ros::spinOnce();
loop_rate.sleep();
// Primero buscamos posicion del cuerpo de Arana
srv_simRosGetObjectPose.request.handle=handleArana;
srv_simRosGetObjectPose.request.relativeToObjectHandle=-1;
if (client_simRosGetObjectPose.call(srv_simRosGetObjectPose)&&(srv_simRosGetObjectPose.response.result!=-1))
{
CuerpoPose = srv_simRosGetObjectPose.response.pose;
// ROS_INFO("posicion: x=%.3f; y=%.3f; z=%.3f\n", CuerpoPose.pose.position.x,CuerpoPose.pose.position.y,CuerpoPose.pose.position.z);
ubicacionRobot.coordenadaCuerpo_x = CuerpoPose.pose.position.x;
ubicacionRobot.coordenadaCuerpo_y = CuerpoPose.pose.position.y;
tf::quaternionMsgToTF(CuerpoPose.pose.orientation,CuerpoOrientacion_Q);
tf::Matrix3x3(CuerpoOrientacion_Q).getRPY(roll, pitch, yaw);
ubicacionRobot.orientacionCuerpo_roll = roll;
ubicacionRobot.orientacionCuerpo_pitch = pitch;
ubicacionRobot.orientacionCuerpo_yaw = yaw;
// ROS_INFO("orientacion: z=%.3f\n", ubicacionRobot.orientacionCuerpo_yaw);
} else {
ROS_ERROR("Nodo 6: servicio de posicion cuerpo no funciona\n");
}
for (k=0;k<Npatas;k++){
ros::spinOnce();
// loop_rate.sleep();
srv_simRosGetObjectPose.request.handle=PataTipHandle[k];
//--------------------------------------------------------------
//---- Obtengo posicion de tip de pata en el mundo
srv_simRosGetObjectPose.request.relativeToObjectHandle=-1;
if (client_simRosGetObjectPose.call(srv_simRosGetObjectPose)&&(srv_simRosGetObjectPose.response.result!=-1))
{
PataTipPose[k] = srv_simRosGetObjectPose.response.pose;
ubicacionRobot.coordenadaPata_y[k] = PataTipPose[k].pose.position.y;
ubicacionRobot.coordenadaPata_x[k] = PataTipPose[k].pose.position.x;
ubicacionRobot.coordenadaPata_z[k] = PataTipPose[k].pose.position.z;
// printf("Nodo6: [%d]: y= %.3f, x= %.3f, z= %.3f\n",k+1,ubicacionRobot.coordenadaPata_y[k],ubicacionRobot.coordenadaPata_x[k],ubicacionRobot.coordenadaPata_z[k]);
srv_simRosReadForceSensor.request.handle=FuerzaSensorHandle[k];
if (client_simRosReadForceSensor.call(srv_simRosReadForceSensor)&&(srv_simRosReadForceSensor.response.result!=-1))
{
//PataTipFuerza=srv_simRosReadForceSensor.response.force;
ubicacionRobot.pataTipFuerza_z[k]=srv_simRosReadForceSensor.response.force.z;
if (ubicacionRobot.pataTipFuerza_z[k]>=umbralFuerzaApoyo) {
//La pata esta en apoyo
ubicacionRobot.pataApoyo[k]=1;
} else {
//La pata esta en transferencia
ubicacionRobot.pataApoyo[k]=0;
}
} else {
ROS_ERROR("Nodo 6: servicio de sensor de fuerza no funciona\n");
ROS_ERROR("Nodo 6: respuesta = %d\n",srv_simRosReadForceSensor.response.result);
}
// printf("Nodo6: Pata apoyo[%d]=%d\n",k,ubicacionRobot.pataApoyo[k]);
} else {
ROS_ERROR("Nodo 6: servicio 1 de posicion pata no funciona\n");
ROS_ERROR("Nodo 6: respuesta = %d\n",srv_simRosGetObjectPose.response.result);
}
}
for (k=0;k<Npatas;k++){
ros::spinOnce();
// loop_rate.sleep();
srv_simRosGetObjectPose.request.handle=PataTipHandle[k];
//--------------------------------------------------------------
//---- Obtengo posicion de tip de pata para cada sistema de pata
srv_simRosGetObjectPose.request.relativeToObjectHandle=Pata_Motor1Handle[k];
if (client_simRosGetObjectPose.call(srv_simRosGetObjectPose)&&(srv_simRosGetObjectPose.response.result!=-1)){
PataTipPose[k] = srv_simRosGetObjectPose.response.pose;
// printf("Pata apoyo[%d]=%d\n",k,ubicacionRobot.pataApoyo[k]);
ubicacionRobot.coordenadaPataSistemaPata_y[k] = PataTipPose[k].pose.position.y;
ubicacionRobot.coordenadaPataSistemaPata_x[k] = PataTipPose[k].pose.position.x;
ubicacionRobot.coordenadaPataSistemaPata_z[k] = PataTipPose[k].pose.position.z;
// printf("Pata: %d: x= %.3f, y= %.3f, z= %.3f\n",k+1,ubicacionRobot.coordenadaPataSistemaPata_x[k],ubicacionRobot.coordenadaPataSistemaPata_y[k],ubicacionRobot.coordenadaPataSistemaPata_z[k]);
} else {
ROS_ERROR("Nodo 6: servicio 2 de posicion pata no funciona\n");
ROS_ERROR("Nodo 6: respuesta = %d\n",srv_simRosGetObjectPose.response.result);
}
}
// ---------------------------------
// fprintf(fp,"\n");
// fprintf(fp,"%.3f\t%.3f\t",ubicacionRobot.coordenadaCuerpo_x,ubicacionRobot.coordenadaCuerpo_y);
// fprintf(fp,"%d\t",cuentaPataApoyo);
// for(k=0;k<Npatas;k++){
// if(pataApoyo[k]==1){
// fprintf(fp,"%.3f\t%.3f\t",ubicacionRobot.coordenadaPata_x[k],ubicacionRobot.coordenadaPata_y[k]);
// }
// }
// fprintf(fp,"\n");
chatter_pub.publish(ubicacionRobot);
}
// fclose(fp);
ROS_INFO("Adios6!");
ros::shutdown();
return(0);
}
void BumperToMTSA::subscriber_init(){
ROS_INFO("subscriber_init");
std::string bumper_topic_name = "/mobile_base/events/bumper";
bumper_sub = nh.subscribe<kobuki_msgs::BumperEvent::ConstPtr>(bumper_topic_name,10, &BumperToMTSA::bumper_callback, this);
pickput_sub = nh.subscribe("chatter",1000,&BumperToMTSA::pickput_callback,this);
}
void BumperToMTSA::run(){
ROS_INFO("run");
subscriber_init();
publisher_init();
}
// Main loop
int main(int argc, char** argv)
{
// ROS Initialization
ros::init(argc, argv, "Roomba_Driver"); // creates ROS node
ros::NodeHandle node; // creates the node handle
ros::Rate loop_rate(100); // testing rate (Hz)
// ROS subscribers
ros::Subscriber joy_sub; // create subscriber to listen to joystick
joy_sub = node.subscribe("joy",1,joy_callback); // this tells our node what to subscribe to, 1 is buffer size of 1
ros::Subscriber track_sub = node.subscribe("UAV_Trackee_RelPos", 1, tracking_callback);
// ROS publishers
/*
ros::Publisher pos_pub = node.advertise<geometry_msgs::Vector3>("position",1);
ros::Publisher yaw_pub = node.advertise<std_msgs::Float32>("yaw",1);
ros::Publisher u_pub = node.advertise<geometry_msgs::Vector3>("control_input",1);
*/
// ROS Service client
ros::ServiceClient client = node.serviceClient<irobot_create_2_1::Tank>("tank");
// Initialize service
irobot_create_2_1::Tank srv;
// loop until time or ros not ok
while(ros::ok())
{
ros::spinOnce(); // Receive callbacks
merge_new_msgs(); // Merge joysticks
if(joy_a) // check error in x
{
merge_new_msgs();
output = mixer(tag_y,0.0);
srv.request.left = output.x;;
srv.request.right= output.y;
srv.request.clear= 1;
client.call(srv);
ROS_INFO("Auto Roomba Track x: %f" ,output.x);
ros::spinOnce();
loop_rate.sleep();
}
/*
if(joy_b) // Stop when B is pressed
start_flag = false;
if(start_flag) // If A has been pressed, run main loop
{
*/
else{
output = mixer(joy_x, joy_y);
// set srv values
srv.request.left = output.x;
srv.request.right= output.y;
srv.request.clear= 1;
// send out value to robot
if(client.call(srv))
{
//ROS_INFO("Sending stuff");
ROS_INFO("Roomba Joy %f %f",output.x,output.y);
//ROS_INFO("service %d %d",srv.request.left,srv.request.right);
if (srv.response.success)
{//ROS_INFO("Response");
}
else
{ROS_INFO("No Response");}
}
else
ROS_INFO("NOT Sending stuff");
}
}//rosok
}//main
//引用传参比较好，不改变值的情况下生明为const安全。
void add_object(const moveit::planning_interface::MoveGroup &group)
{
ros::NodeHandle node_handle;
//添加物体
// Advertise the required topic
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Note that this topic may need to be remapped in the launch file
ros::Publisher planning_scene_diff_publisher = node_handle.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
while(planning_scene_diff_publisher.getNumSubscribers() < 1)
{
ros::WallDuration sleep_t(0.5);
sleep_t.sleep();
}
// Define the attached object message
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// We will use this message to add or
// subtract the object from the world
// and to attach the object to the robot
moveit_msgs::AttachedCollisionObject attached_object;
attached_object.link_name = "lLink7";
/* The header must contain a valid TF frame*/
attached_object.object.header.frame_id = group.getPlanningFrame();
/* The id of the object */
attached_object.object.id = "box1";
/* A default left pose */
geometry_msgs::Pose pose1;
pose1.orientation.w = 1.0;
pose1.position.x=0.4;
pose1.position.y=0.64;
pose1.position.z=0.6;
/* Define a left box to be attached */
shape_msgs::SolidPrimitive primitive1;
primitive1.type = primitive1.BOX;
primitive1.dimensions.resize(3);
primitive1.dimensions[0] = 0.03;
primitive1.dimensions[1] = 0.03;
primitive1.dimensions[2] = 0.2;
/* A default right pose */
geometry_msgs::Pose pose2;
pose2.orientation.w = 1.0;
pose2.position.x=-0.4;
pose2.position.y=0.7;
pose2.position.z=0.4;
/* Define a right box to be attached */
shape_msgs::SolidPrimitive primitive2;
primitive2.type = primitive2.BOX;
primitive2.dimensions.resize(3);
primitive2.dimensions[0] = 0.3;
primitive2.dimensions[1] = 0.3;
primitive2.dimensions[2] = 0.4;
//容器使用push_back进行添加元素
attached_object.object.primitives.push_back(primitive1);
attached_object.object.primitive_poses.push_back(pose1);
attached_object.object.primitives.push_back(primitive2);
attached_object.object.primitive_poses.push_back(pose2);
// Note that attaching an object to the robot requires
// the corresponding operation to be specified as an ADD operation
attached_object.object.operation = attached_object.object.ADD;
// Add an object into the environment
// ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
// Add the object into the environment by adding it to
// the set of collision objects in the "world" part of the
// planning scene. Note that we are using only the "object"
// field of the attached_object message here.
ROS_INFO("Adding the object into the world ");
moveit_msgs::PlanningScene planning_scene;
planning_scene.world.collision_objects.push_back(attached_object.object);
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
sleep(2);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "dual_interactive_self");
ros::AsyncSpinner spinner(1);
spinner.start();
//sleep(20.0);//等rviz起来，如果没在一个lanuch中，可以省去这个
moveit::planning_interface::MoveGroup group("dual_arms");
// Getting Basic Information
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());//打印坐标系
ROS_INFO("Reference frame: %s", group.getEndEffectorLink().c_str());//打印末端链节
add_object(group);//添加左右两个物体
geometry_msgs::PoseStamped current_pose = group.getCurrentPose("lLink7");
ROS_INFO("current left pose:x=%f,y=%f,z=%f",current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
ROS_INFO("current let qurternion:x=%f,y=%f,z=%f,w=%f",current_pose.pose.orientation.x,current_pose.pose.orientation.y,current_pose.pose.orientation.z,current_pose.pose.orientation.w);
std::cout<<"左臂初始RPY=";
float r,p,y;
xyzw_to_rpy(-1,0,0,0,r,p,y);
current_pose = group.getCurrentPose("rLink7");
ROS_INFO("current right pose:x=%f,y=%f,z=%f",current_pose.pose.position.x,current_pose.pose.position.y,current_pose.pose.position.z);
ROS_INFO("current right quaternion:x=%f,y=%f,z=%f,w=%f",current_pose.pose.orientation.x,current_pose.pose.orientation.y,current_pose.pose.orientation.z,current_pose.pose.orientation.w);
std::cout<<"右臂初始RPY=";
xyzw_to_rpy(-1,0,0,0,r,p,y);
//给左臂末端赋值
geometry_msgs::Pose testPose1 = current_pose.pose;
geometry_msgs::Pose pose1;//和上面添加物体函数中给水杯添加的位置一样
pose1.orientation.w = 1.0;
pose1.position.x=0.4;
pose1.position.y=0.64;
pose1.position.z=0.6;
testPose1.position = pose1.position;
testPose1.position.y = pose1.position.y - 0.14;//不能碰杯子否则规划失败，所以留出一些空间
std::cout<<"左臂goal的欧拉角：";
xyzw_to_rpy(0.699477,0.714655,0.000155,0.000255,r,p,y);//显示下左臂goal的欧拉角
std::cout<<"水杯的欧拉角";
xyzw_to_rpy(pose1.orientation.x,pose1.orientation.y,pose1.orientation.z,pose1.orientation.w,r,p,y);//得到水杯的欧拉角
rpy_to_xyzw(3.14+r,0+p,1.6+y,testPose1);
std::cout<<"左臂goal的四元数：";
std::cout<<testPose1.orientation.x<<testPose1.orientation.y<<testPose1.orientation.z<<testPose1.orientation.w<<std::endl;
//给右臂末端赋值
geometry_msgs::Pose testPose2 = current_pose.pose;
testPose2.position.x = -0.385743;
testPose2.position.y = 0.467167;
testPose2.position.z = 0.674107;
// testPose2.orientation.x = 0.702235;
// testPose2.orientation.y =-0.710211;
// testPose2.orientation.z = -0.033619;
// testPose2.orientation.w = 0.036564;
xyzw_to_rpy(0.70223,-0.710211,-0.03361,0.036564,r,p,y);
rpy_to_xyzw(3.14,0,-1.5,testPose2);
std::cout<<testPose2.orientation.x<<testPose2.orientation.y<<testPose2.orientation.z<<testPose2.orientation.w<<std::endl;
//设定home位置
std::vector<double> group_variable_values;
group_variable_values = group.getCurrentJointValues();
//c++98不支持，c++11新特性
//for(double d : group_variable_values){
//std::cout << d << std::endl;
// }
std::cout<<"打印初始位姿的各个关节角度值:"<<std::endl;
std::vector<double>::iterator d = group_variable_values.begin();
while(d != group_variable_values.end()) {
std::cout << *(d++) << std::endl;
}
for(int i=0; i<14; i++){
group_variable_values[i] = 0.0;
}
//group.setMaxVelocityScalingFactor(0.1);
//规划运动到目标位置
group.setPoseTarget(testPose1,"lLink7");
group.setPoseTarget(testPose2,"rLink7");
//group.asyncMove();
moveit::planning_interface::MoveGroup::Plan plan_goal;
group.plan(plan_goal);
group.asyncExecute(plan_goal);
//通过命令去控制规划到home还是goal位置
std::string s1 = "go home";
std::string command;
std::string s2 = "go to the goal";
moveit::planning_interface::MoveGroup::Plan plan_home;
while(1){
std::cout << "Please input command(Eg:go home,go to the goal):";
std::getline(std::cin, command);
if(command == s1){
group.setJointValueTarget(group_variable_values);
group.plan(plan_home);
group.asyncExecute(plan_home);
}else if(command == s2){
group.setPoseTarget(testPose1,"lLink7");
group.setPoseTarget(testPose2,"rLink7");
group.asyncMove();
}else{
ROS_INFO("command is invalid, please again");
}
}
ros::shutdown();
return 0;
}
int ConfigManager::readConfigFile(int fd, std::istream* stream)
{
YAML::Parser p;
YAML::Node doc;
int ret = 0;
std::vector<v4l2_ext_control> controls;
try
{
p.Load(*stream);
}
catch(YAML::Exception& e)
{
ROS_FATAL("Could not parse config file: %s", e.what());
return -1;
}
if(!p.GetNextDocument(doc))
{
ROS_FATAL("Could not get YAML document");
return -1;
}
for(YAML::Iterator it = doc.begin(); it != doc.end(); ++it)
{
std::string key, value;
try
{
(*it).begin().first() >> key;
(*it).begin().second() >> value;
}
catch(YAML::Exception& e)
{
ROS_FATAL("Invalid parameter specification: %s", e.what());
return -1;
}
int ivalue = -1;
calibration::CameraParam* info = getParamInfo(key);
if(!info)
{
ROS_FATAL("Unknown parameter '%s' in config file, ignoring...", key.c_str());
continue;
}
ivalue = atoi(value.c_str());
info->value = ivalue;
v4l2_ext_control control;
memset(&control, 0, sizeof(control));
control.id = info->id;
control.value = ivalue;
controls.push_back(control);
}
v4l2_ext_controls request;
memset(&request, 0, sizeof(request));
request.controls = &controls[0];
request.count = controls.size();
request.ctrl_class = V4L2_CTRL_CLASS_USER;
if(ioctl(fd, VIDIOC_S_EXT_CTRLS, &request) != 0)
{
ROS_FATAL("Control setting failed. This may have happened at control 0x%X with value %d",
controls[request.error_idx].id,
controls[request.error_idx].value
);
return -1;
}
ROS_INFO("Controls set successfully from config file");
return ret;
}
//---------------------------------------------------------------------------------------------------
// Can get setPiont from launch file, command line, or another publisher--this yet to be implemented.
// setPoint looks like:
//---------------------------------------------------------------------------------------------------
// #Number of Controllers
// int32 num_ctrls
//
// # Name of the type(s) of Controller(s): force or moment
// string[] type
//
// # Desired force/moment (3D) for dominant controller
// geometry_msgs/Vector3[] domDes
//
// # Gains force/moment (3D) for dominant controller
// geometry_msgs/Vector3[] domGains
//
// # Desired force/moment (3D) for subordinate controller
// geometry_msgs/Vector3[] subDes
//
// # Gains force/moment (3D) for subordinate controller
// geometry_msgs/Vector3[] subGains
//---------------------------------------------------------------------------------------------------
// Command line arguments can be expected in any of the following forms:
// numCtrls mode1 desx1 desy1 desz1 # argc= 6 => 1 controller with setpoint
// numCtrls mode1 desx1 desy1 desz1 gx1 gy1 gz1 # argc= 9 => 1 controller with setpoint and mode
//---------------------------------------------------------------------------------------------------
// numCtrls mode1 desx1 desy1 desz1 mode2 desx2 desy2 desz2 # argc=10 => 2 controller with setpoints
// numCtrls mode1 desx1 desy1 desz1 gx1 gx2 gx3 mode2 desx2 desy2 desz2 # argc=13 => 2 controller 1st one with setpoints and gains, 2nd one only with setpoint
// numCtrls mode1 desx1 desy1 desz1 gx1 gx2 gx3 mode2 desx2 desy2 desz2 gx2 gy2 gz2 # argc=16 => 2 controller with setpoints and gains
//---------------------------------------------------------------------------------------------------
int main(int argc, char **argv)
{
// Start the node
ros::init(argc, argv, "setPoint_publisher");
ros::NodeHandle n;
// A. Handle command line arguments
int numCtrls=1;
int mode1=0, mode2=0;
double x1=0, x2=0;
double y1=0, y2=0;
double z1=0, z2=0;
double gx1=0, gx2=0;
double gy1=0, gy2=0;
double gz1=0, gz2=0;
/*** Variable Initialization according to user input ***/
// User only entered desired setpoint.
numCtrls=atof(argv[1]);
// Dom Controller with SetPoint
if(argc==6 && numCtrls==1)
{
mode1=atof(argv[2]);
x1 =atof(argv[3]);
y1 =atof(argv[4]);
z1 =atof(argv[5]);
}
// Dom Controller with SetPoint and Gains
else if(argc==9 && numCtrls==1)
{
mode1=atof(argv[2]);
x1 =atof(argv[3]);
y1 =atof(argv[4]);
z1 =atof(argv[5]);
gx1 =atof(argv[6]);
gy1 =atof(argv[7]);
gz1 =atof(argv[8]);
}
// Dom and Sub Controller with SetPoint
else if(argc==10 && numCtrls==2)
{
mode1=atof(argv[2]);
x1 =atof(argv[3]);
y1 =atof(argv[4]);
z1 =atof(argv[5]);
mode2=atof(argv[6]);
x2 =atof(argv[7]);
y2 =atof(argv[8]);
z2 =atof(argv[9]);
}
// Dom Controller with SetPoint and Gains and SubController with SetPoint
else if(argc==13 && numCtrls==2)
{
mode1=atof(argv[2]);
x1 =atof(argv[3]);
y1 =atof(argv[4]);
z1 =atof(argv[5]);
gx1 =atof(argv[6]);
gy1 =atof(argv[7]);
gz1 =atof(argv[8]);
mode2=atof(argv[9]);
x2 =atof(argv[10]);
y2 =atof(argv[11]);
z2 =atof(argv[12]);
}
// Both Controllers with SetPoint and Gains
else if(argc==16 && numCtrls==2)
{
mode1=atof(argv[2]);
x1 =atof(argv[3]);
y1 =atof(argv[4]);
z1 =atof(argv[5]);
gx1 =atof(argv[6]);
gy1 =atof(argv[7]);
gz1 =atof(argv[8]);
mode2=atof(argv[9]);
x2 =atof(argv[10]);
y2 =atof(argv[11]);
z2 =atof(argv[12]);
gx2 =atof(argv[13]);
gy2 =atof(argv[14]);
gz2 =atof(argv[15]);
}
else
{
ROS_INFO("Wrong number of arguments in the commmand line.");
}
// B. Initialize ROS data
// Get side Parameter
std::string side_="";
n.param<std::string>("side", side_, "right");
// Create the publisher
ros::Publisher setPoint_pub = n.advertise<force_controller::setPoint>("/" + side_ + "/force_control/setPoint",1);
// Populate the setPoint msg
force_controller::setPoint sP;
// Set Control Type, Desired set-point and gains for 1 or 2 controllers.
if(numCtrls < 1 || numCtrls > 2)
{
ROS_ERROR("Wrong number of controllers given");
return -1;
}
else
{
// Number of Controllers
sP.num_ctrls=numCtrls;
/********** Dominant Controller ************/
/*** CONTROL TYPE ***/
// Dominant Controller Type
if(mode1==0) sP.domType="force";
else if(mode1==1) sP.domType="moment";
/*** SETPOINT ***/
geometry_msgs::Vector3 d;
d.x=x1; d.y=y1; d.z=z1;
sP.domDes.push_back(d);
/*** GAINS ***/
if(argc==9 || argc==13 || argc==16) {
d.x=gx1; d.y=gy1; d.z=gz1;
sP.domGains.push_back(d);
}
/************ Subordinate Controller **********/
if(numCtrls==2) // If condition not true it's okay if all are empty
{
/*** CONTROL TYPE ***/
// Subordinate Controller Type
if(mode2==0) sP.subType="force";
else if(mode2==1) sP.subType="moment";
/*** SETPOINT ***/
d.x=x2; d.y=y2; d.z=z2;
sP.subDes.push_back(d);
/*** Gains ***/
if(argc==16) {
d.x=gx2; d.y=gy2; d.z=gz2;
sP.subGains.push_back(d);
}
}
}
// Set ROS Rate for while loop
ros::Rate rate(RATE);
while(ros::ok())
{
setPoint_pub.publish(sP);
ros::spinOnce();
rate.sleep();
}
return 0;
}
int ConfigManager::init(int fd, const std::vector<std::string>& controls)
{
v4l2_queryctrl q;
memset (&q, 0, sizeof (q));
m_controls.params.clear();
m_controls.params.resize(controls.size());
q.id = V4L2_CTRL_FLAG_NEXT_CTRL;
while(1)
{
if(ioctl(fd, VIDIOC_QUERYCTRL, &q) != 0)
{
if(errno == EINVAL)
break;
ROS_FATAL("Could not query control: %d: %s", errno, strerror(errno));
return -1;
}
if (q.flags & V4L2_CTRL_FLAG_DISABLED)
{
ROS_INFO("skipping disabled control '%s'", (const char*)q.name);
continue;
}
v4l2_control control;
memset(&control, 0, sizeof(control));
control.id = q.id;
if(ioctl(fd, VIDIOC_G_CTRL, &control) != 0)
{
ROS_FATAL("Could not get control value");
return -1;
}
// ROS_INFO("(id=0x%X) Control '%s' (type %s): %d", q.id, q.name, v4l2_type_to_str(q.type), control.value);
calibration::CameraParam param;
param.id = q.id;
param.label = (const char*)q.name;
param.maximum = q.maximum;
param.minimum = q.minimum;
param.value = control.value;
if(q.type == V4L2_CTRL_TYPE_MENU)
{
if(enumerateMenu(fd, ¶m, q) != 0)
{
ROS_FATAL("Could not query menu");
return -1;
}
}
std::vector<std::string>::const_iterator it = std::find(
controls.begin(), controls.end(), param.label
);
if(it != controls.end())
{
int idx = it - controls.begin();
m_controls.params[idx] = param;
}
else
ROS_INFO("Ignoring unknown parameter '%s'", q.name);
q.id |= V4L2_CTRL_FLAG_NEXT_CTRL;
}
bool error = false;
for(size_t i = 0; i < m_controls.params.size(); ++i)
{
if(m_controls.params[i].label == "")
{
ROS_ERROR("Could not find whitelisted camera parameter '%s'", controls[i].c_str());
error = true;
}
}
if(error)
return -1;
return 0;
}
//printing the asynchronous data;
void asyncDataListener(Vn100* vn100,Vn100CompositeData* data)
{
ros::Time timestamp=ros::Time::now();
static int seq =0;
ROS_INFO("\nASYNC DATA:\n"
"YPR.Yaw: %+#7.2f\n"
"YPR.pitch: %+#7.2f\n"
"YPR.roll: %+#7.2f\n",
data->ypr.yaw,
data->ypr.pitch,
data->ypr.roll);
ROS_INFO(
"\n quaternion.X: %+#7.2f\n"
" quaternion.Y: %+#7.2f\n"
" quaternion.Z: %+#7.2f\n"
" quaternion.W: %+#7.2f\n",
data->quaternion.x,
data->quaternion.y,
data->quaternion.z,
data->quaternion.w);
ROS_INFO(
"\n {Value,Voltage}\n"
" magnetic X: %+#7.2f, %+#7.2f\n"
" magnetic Y: %+#7.2f, %+#7.2f\n"
" magnetic Z: %+#7.2f, %+#7.2f\n",
data->magnetic.c0,data->magneticVoltage.c0,
data->magnetic.c1,data->magneticVoltage.c1,
data->magnetic.c2,data->magneticVoltage.c2);
ROS_INFO(
"\n acceleration X: %+#7.2f,%+#7.2f\n"
" acceleration Y: %+#7.2f,%+#7.2f\n"
" acceleration Z: %+#7.2f,%+#7.2f\n",
data->acceleration.c0,
data->accelerationVoltage.c0,
data->acceleration.c1,
data->accelerationVoltage.c1,
data->acceleration.c2,
data->accelerationVoltage.c2);
ROS_INFO(
"\n {Value ,Voltage,Bias,BiasVariance}"
" angularRate X: %+#7.2f, %+#7.2f, %+#7.2f, %+#7.2f\n"
" angularRate Y: %+#7.2f, %+#7.2f, %+#7.2f, %+#7.2f\n"
" angularRate Z: %+#7.2f, %+#7.2f, %+#7.2f, %+#7.2f\n",
data->angularRate.c0,
data->angularRateVoltage.c0,
data->angularRateBias.c0,
data->angularRateBiasVariance.c0,
data->angularRate.c1,
data->angularRateVoltage.c1,
data->angularRateBias.c1,
data->angularRateBiasVariance.c1,
data->angularRate.c2,
data->angularRateVoltage.c2,
data->angularRateBias.c2, data->angularRateBiasVariance.c2);
ROS_INFO(
"\n Attitude Variance X: %+#7.2f\n"
" Attitude Variance Y: %+#7.2f\n"
" Attitude Variance Z: %+#7.2f\n",
data->attitudeVariance.c0,
data->attitudeVariance.c1,
data->attitudeVariance.c2);
ROS_INFO(
"\n Direction Cosine Matrix:\n"
" %+#7.2f, %+#7.2f, %+#7.2f\n"
" %+#7.2f, %+#7.2f, %+#7.2f\n"
" %+#7.2f, %+#7.2f, %+#7.2f\n",
data->dcm.c00, data->dcm.c01, data->dcm.c02,
data->dcm.c10, data->dcm.c11, data->dcm.c12,
data->dcm.c20, data->dcm.c21, data->dcm.c22);
ROS_INFO(
"\n Temperature: %+#7.2f\n"
" Temperature Voltage: %+#7.2f\n",
data->temperature,
data->temperatureVoltage);
}
bool I2cImu::ImuSettings::loadSettings()
{
ROS_INFO("%s: reading IMU parameters from param server", __FUNCTION__);
int temp_int;
// General
settings_nh_->getParam("imu_type", m_imuType);
settings_nh_->getParam("fusion_type", m_fusionType);
if(settings_nh_->getParam("i2c_bus", temp_int))
m_I2CBus = (unsigned char) temp_int;
if(settings_nh_->getParam("i2c_slave_address", temp_int))
m_I2CSlaveAddress = (unsigned char) temp_int;
//double declination_radians;
//settings_nh_->param("magnetic_declination", declination_radians, 0.0);
//m_compassAdjDeclination = angles::to_degrees(declination_radians);
//MPU9150
settings_nh_->getParam("mpu9150/gyro_accel_sample_rate", m_MPU9150GyroAccelSampleRate);
settings_nh_->getParam("mpu9150/compass_sample_rate", m_MPU9150CompassSampleRate);
settings_nh_->getParam("mpu9150/accel_full_scale_range", m_MPU9150AccelFsr);
settings_nh_->getParam("mpu9150/gyro_accel_low_pass_filter", m_MPU9150GyroAccelLpf);
settings_nh_->getParam("mpu9150/gyro_full_scale_range", m_MPU9150GyroFsr);
//MPU9250
settings_nh_->getParam("mpu9250/gyro_accel_sample_rate", m_MPU9250GyroAccelSampleRate);
settings_nh_->getParam("mpu9250/compass_sample_rate", m_MPU9250CompassSampleRate);
settings_nh_->getParam("mpu9250/accel_full_scale_range", m_MPU9250AccelFsr);
settings_nh_->getParam("mpu9250/accel_low_pass_filter", m_MPU9250AccelLpf);
settings_nh_->getParam("mpu9250/gyro_full_scale_range", m_MPU9250GyroFsr);
settings_nh_->getParam("mpu9250/gyro_low_pass_filter", m_MPU9250GyroLpf);
//GD20HM303D
settings_nh_->getParam("GD20HM303D/gyro_sample_rate", m_GD20HM303DGyroSampleRate);
settings_nh_->getParam("GD20HM303D/accel_sample_rate", m_GD20HM303DAccelSampleRate);
settings_nh_->getParam("GD20HM303D/compass_sample_rate", m_GD20HM303DCompassSampleRate);
settings_nh_->getParam("GD20HM303D/accel_full_scale_range", m_GD20HM303DAccelFsr);
settings_nh_->getParam("GD20HM303D/gyro_full_scale_range", m_GD20HM303DGyroFsr);
settings_nh_->getParam("GD20HM303D/compass_full_scale_range", m_GD20HM303DCompassFsr);
settings_nh_->getParam("GD20HM303D/accel_low_pass_filter", m_GD20HM303DAccelLpf);
settings_nh_->getParam("GD20HM303D/gyro_high_pass_filter", m_GD20HM303DGyroHpf);
settings_nh_->getParam("GD20HM303D/gyro_bandwidth", m_GD20HM303DGyroBW);
//GD20M303DLHC
settings_nh_->getParam("GD20M303DLHC/gyro_sample_rate",m_GD20M303DLHCGyroSampleRate);
settings_nh_->getParam("GD20M303DLHC/accel_sample_rate",m_GD20M303DLHCAccelSampleRate);
settings_nh_->getParam("GD20M303DLHC/compass_sample_rate",m_GD20M303DLHCCompassSampleRate);
settings_nh_->getParam("GD20M303DLHC/accel_full_scale_range",m_GD20M303DLHCAccelFsr);
settings_nh_->getParam("GD20M303DLHC/gyro_full_scale_range",m_GD20M303DLHCGyroFsr);
settings_nh_->getParam("GD20M303DLHC/compass_full_scale_range",m_GD20M303DLHCCompassFsr);
settings_nh_->getParam("GD20M303DLHC/gyro_high_pass_filter",m_GD20M303DLHCGyroHpf);
settings_nh_->getParam("GD20M303DLHC/gyro_bandwidth",m_GD20M303DLHCGyroBW);
//GD20HM303DLHC
settings_nh_->getParam("GD20HM303DLHC/gyro_sample_rate", m_GD20HM303DLHCGyroSampleRate);
settings_nh_->getParam("GD20HM303DLHC/accel_sample_rate",m_GD20HM303DLHCAccelSampleRate);
settings_nh_->getParam("GD20HM303DLHC/compass_sample_rate",m_GD20HM303DLHCCompassSampleRate);
settings_nh_->getParam("GD20HM303DLHC/accel_full_scale_range",m_GD20HM303DLHCAccelFsr);
settings_nh_->getParam("GD20HM303DLHC/gyro_full_scale_range",m_GD20HM303DLHCGyroFsr);
settings_nh_->getParam("GD20HM303DLHC/compass_full_scale_range",m_GD20HM303DLHCCompassFsr);
settings_nh_->getParam("GD20HM303DLHC/gyro_high_pass_filter",m_GD20HM303DLHCGyroHpf);
settings_nh_->getParam("GD20HM303DLHC/gyro_bandwidth",m_GD20HM303DLHCGyroBW);
//LSM9DS0
settings_nh_->getParam("LSM9DS0/gyro_sample_rate",m_LSM9DS0GyroSampleRate);
settings_nh_->getParam("LSM9DS0/accel_sample_rate",m_LSM9DS0AccelSampleRate);
settings_nh_->getParam("LSM9DS0/compass_sample_rate",m_LSM9DS0CompassSampleRate);
settings_nh_->getParam("LSM9DS0/accel_full_scale_range",m_LSM9DS0AccelFsr);
settings_nh_->getParam("LSM9DS0/gyro_full_scale_range",m_LSM9DS0GyroFsr);
settings_nh_->getParam("LSM9DS0/compass_full_scale_range",m_LSM9DS0CompassFsr);
settings_nh_->getParam("LSM9DS0/accel_low_pass_filter",m_LSM9DS0AccelLpf);
settings_nh_->getParam("LSM9DS0/gyro_high_pass_filter",m_LSM9DS0GyroHpf);
settings_nh_->getParam("LSM9DS0/gyro_bandwidth",m_LSM9DS0GyroBW);
std::vector<double> compass_max, compass_min;
if (settings_nh_->getParam("calib/compass_min", compass_min)
&& settings_nh_->getParam("calib/compass_max", compass_max)
&& compass_min.size() == 3 && compass_max.size() == 3)
{
m_compassCalMin = RTVector3(compass_min[0], compass_min[1], compass_min[2]);
m_compassCalMax = RTVector3(compass_max[0],compass_max[1], compass_max[2]);
m_compassCalValid = true;
ROS_INFO("Got Calibration for Compass");
}
else
{
ROS_INFO("No Calibration for Compass");
}
std::vector<double> accel_max, accel_min;
if (settings_nh_->getParam("calib/accel_min", accel_min)
&& settings_nh_->getParam("calib/accel_max", accel_max)
&& accel_min.size() == 3 && accel_max.size() == 3)
{
m_accelCalMin = RTVector3(accel_min[0], accel_min[1], accel_min[2]);
m_accelCalMax = RTVector3(accel_max[0],accel_max[1], accel_max[2]);
m_accelCalValid = true;
ROS_INFO("Got Calibration for Accelerometer");
}
else
{
ROS_INFO("No Calibration for Accelerometer");
}
return true;
}
int main(int argc,char** argv)
{
ros::init(argc,argv,"vn_100"); ////initializing ros node
ros::NodeHandle n;
ros::NodeHandle np("~"); //creating public and private nodehandlers to ha ndle ros publish services and private parameters
std::string port;
int baudrate,publish_rate,async_output_rate,async_output_type;
np.param<std::string>("serial_port",port,"/dev/ttyUSB0");
np.param<int>("serial_baud",baudrate,115200);
np.param<int>("publish_rate",publish_rate,10);
np.param<int>("async_output_type",async_output_type,0);
np.param<int>("async_output_rate",async_output_rate,6);//assigning params to variables
pubsens_data =np.advertise<vn_100::sensor_data> ("sensor_data",1);
pubins_data =np.advertise<vn_100::ins_data> ("ins_data",1);//Initializing Publishers
ros::ServiceServer service=n.advertiseService("query_ins_data",send_data);
ROS_INFO("Ready to answer your queries regarding ins data");
VN_ERROR_CODE vn_err; //dealing with vectornav errors
std::string vn_error_msg;
ROS_INFO("connecting to vn100. port: %s at a baudrate:%d\n",
port.c_str(),
baudrate);
vn_err=vn100_connect(&vn100,port.c_str(),baudrate);//connecting to vectornav
if(vn_err!=VNERR_NO_ERROR) //in case of any error
{
vnerror_msg(vn_err,vn_error_msg);
ROS_FATAL("Could not connect to the sensor on this %s port error:%s\n did you ad d the user to the dialout group???",
port.c_str(),
vn_error_msg.c_str()
);
exit(EXIT_FAILURE);
}
vn_err=vn100_setAsynchronousDataOutputType(&vn100,async_output_type,true);
if(vn_err!=VNERR_NO_ERROR)
{
vnerror_msg(vn_err,vn_error_msg);
ROS_FATAL("Could not set the output mode error:%s",
vn_error_msg.c_str());
exit(EXIT_FAILURE);
}
//enabling ros timer to publish the data at the particular intervals
ros::Timer pub_timer;
if(async_output_type ==0)
{
//publishing loop
ROS_INFO("publishing at %d Hz\n",publish_rate);
pub_timer=np.createTimer(ros::Duration(1.0/(double)publish_rate),publish_timer);
}
else
{
if (async_output_rate==1|async_output_rate==2|async_output_rate==4|async_output_rate==5|async_output_rate==10|async_output_rate==20|async_output_rate==25|async_output_rate==40|async_output_rate==50|async_output_rate==100|async_output_rate==200)
{
ROS_INFO("asynchronous output is subscribed at %d Hz\n",async_output_rate);
}
else
{
ROS_ERROR("Invalid Async rate %d Hz\n Valid rates:{1,2,4,5,10,25,40,50,100,200}",async_output_rate);
}
}
vn100_setAsynchronousDataOutputFrequency(&vn100,async_output_rate,true);
vn100_registerAsyncDataReceivedListener(&vn100,&asyncDataListener);
ros::spin();
vn100_disconnect(&vn100);
return 0;
}
void point_cb(const sensor_msgs::PointCloud2ConstPtr& cloud_msg){
pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2;
pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
pcl::PCLPointCloud2 cloud_filtered;
pcl_conversions::toPCL(*cloud_msg, *cloud);
pcl::VoxelGrid<pcl::PCLPointCloud2> sor;
sor.setInputCloud(cloudPtr);
float leaf = 0.005;
sor.setLeafSize(leaf, leaf, leaf);
sor.filter(cloud_filtered);
sensor_msgs::PointCloud2 sensor_pcl;
pcl_conversions::moveFromPCL(cloud_filtered, sensor_pcl);
//publish pcl data
pub_voxel.publish(sensor_pcl);
global_counter++;
if((theta == 0.0 && y_offset == 0.0) || global_counter < 800 ){
// part for planar segmentation starts here ..
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud1(new pcl::PointCloud<pcl::PointXYZ>), cloud_p(new pcl::PointCloud<pcl::PointXYZ>), cloud_seg(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_rotated(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_p_transformed(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_transformed(new pcl::PointCloud<pcl::PointXYZ>);
sensor_msgs::PointCloud2 plane_sensor, plane_transf_sensor;
//convert sen
pcl::fromROSMsg(*cloud_msg, *cloud1);
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
pcl::SACSegmentation<pcl::PointXYZ> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType (pcl::SACMODEL_PLANE);
seg.setMethodType (pcl::SAC_RANSAC);
seg.setMaxIterations (100);
seg.setDistanceThreshold (0.01);
seg.setInputCloud(cloud1);
seg.segment (*inliers, *coefficients);
Eigen::Matrix<float, 1, 3> surface_normal;
Eigen::Matrix<float, 1, 3> floor_normal;
surface_normal[0] = coefficients->values[0];
surface_normal[1] = coefficients->values[1];
surface_normal[2] = coefficients->values[2];
std::cout << surface_normal[0] << "\n" << surface_normal[1] << "\n" << surface_normal[2];
floor_normal[0] = 0.0;
floor_normal[1] = 1.0;
floor_normal[2] = 0.0;
theta = acos(surface_normal.dot(floor_normal));
//extract the inliers - copied from tutorials...
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(cloud1);
extract.setIndices (inliers);
extract.setNegative(true);
extract.filter(*cloud_p);
ROS_INFO("print cloud info %d", cloud_p->height);
pcl::toROSMsg(*cloud_p, plane_sensor);
pub_plane_simple.publish(plane_sensor);
// anti rotate the point cloud by first finding the angle of rotation
Eigen::Affine3f transform_1 = Eigen::Affine3f::Identity();
transform_1.translation() << 0.0, 0.0, 0.0;
transform_1.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_rotated, transform_1);
double y_sum = 0;
// int num_points =
for (int i = 0; i < 20; i++){
y_sum = cloud_p_rotated->points[i].y;
}
y_offset = y_sum / 20;
Eigen::Affine3f transform_2 = Eigen::Affine3f::Identity();
transform_2.translation() << 0.0, -y_offset, 0.0;
transform_2.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitX()));
pcl::transformPointCloud(*cloud_p, *cloud_p_transformed, transform_2);
pcl::transformPointCloud(*cloud1, *cloud_transformed, transform_2);
//now remove the y offset because of the camera rotation
pcl::toROSMsg(*cloud_p_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud_transformed, plane_transf_sensor);
// pcl::toROSMsg(*cloud1, plane_transf_sensor);
pub_plane_transf.publish(plane_transf_sensor);
}
ras_msgs::Cam_transform cft;
cft.theta = theta;
cft.y_offset = y_offset;
pub_ctf.publish(cft);
// pub_tf.publish();
}
void publish_device()
{
static int seq=0;
seq++;
VN_ERROR_CODE vn_error;
std::string vn_err_msg;
ros::Time timestamp=ros::Time::now();
if(pubsens_data.getNumSubscribers()>0)
{
ROS_INFO("subscribed");
VnVector3 Gyro,Mag,Accel;
float Pressure,Temp;
vn_error=vn100_getCalibratedSensorMeasurements(&vn100,&Mag,&Accel,&Gyro,&Temp,&Pressure);
if(vn_error!=VNERR_NO_ERROR)
{
vnerror_msg(vn_error,vn_err_msg);
ROS_INFO("error in reading the sensor data:%s",vn_err_msg.c_str());
}
else
{
vn_100::sensor_data msg_sensor_data;
msg_sensor_data.header.seq=seq;
msg_sensor_data.header.stamp=timestamp;
msg_sensor_data.header.frame_id=imu_frame_id;
msg_sensor_data.Mag.x=Mag.c0;
msg_sensor_data.Mag.y=Mag.c1;
msg_sensor_data.Mag.z=Mag.c2;
msg_sensor_data.Accel.x=Accel.c0;
msg_sensor_data.Accel.y=Accel.c1;
msg_sensor_data.Accel.z=Accel.c2;
msg_sensor_data.Gyro.x=Gyro.c0;
msg_sensor_data.Gyro.y=Gyro.c1;
msg_sensor_data.Gyro.z=Gyro.c2;
msg_sensor_data.Temp=Temp;
msg_sensor_data.Pressure=Pressure;
pubsens_data.publish(msg_sensor_data);
}
}
if(pubins_data.getNumSubscribers()>0)
{
VnYpr YPR;
VnQuaternion quat_data;
vn_100::ins_data msg_ins_data;
vn_error=vn100_getYawPitchRoll(&vn100,&YPR);
if(vn_error!=VNERR_NO_ERROR)
{
vnerror_msg(vn_error,vn_err_msg);
ROS_INFO("error in reading the ins data:%s",vn_err_msg.c_str());
}
else
{
vn_error=vn100_getQuaternion(&vn100,&quat_data);
if(vn_error!=VNERR_NO_ERROR)
{
vnerror_msg(vn_error,vn_err_msg);
ROS_INFO("error in reading the ins data:%s",vn_err_msg.c_str());
}
else
{
msg_ins_data.header.seq=seq;
msg_ins_data.header.stamp=timestamp;
msg_ins_data.header.frame_id=imu_frame_id;
msg_ins_data.YPR.x=YPR.yaw;
msg_ins_data.YPR.y=YPR.pitch;
msg_ins_data.YPR.z=YPR.roll;
msg_ins_data.quat_data[0]=quat_data.x;
msg_ins_data.quat_data[1]=quat_data.y;
msg_ins_data.quat_data[2]=quat_data.z;
msg_ins_data.quat_data[3]=quat_data.w;
pubins_data.publish(msg_ins_data);
}
}
}
}
int ErrorHandler(CPhidgetHandle phid, void *userptr, int ErrorCode, const char *Description)
{
ROS_INFO("Error handled. %d - %s", ErrorCode, Description);
return 0;
}
/**
* Independent thread that grabs images and publishes them.
*/
void StereoCamera::feedImages() {
unsigned int pair_id = 0;
ROS_INFO("feedImages");
cv::Mat left, right;
// ok is a global that goes false in destructor
while (ok) {
//unsigned char *frame_left = NULL, *frame_right = NULL;
//uint32_t bytes_used_left, bytes_used_right;
ros::Time capture_time = ros::Time::now();
//int left_idx = cam_left->grab(&frame_left, bytes_used_left);
//int right_idx = cam_right->grab(&frame_right, bytes_used_right);
left = cam_left->getImage();
right = cam_left->getImage();
// Read in every frame the camera generates, but only send each
// (skip_frames + 1)th frame. This reduces effective frame
// rate, processing time and network usage while keeping the
// images synchronized within the true framerate.
//
if (skip_frames == 0 || frames_to_skip == 0) {
//if (frame_left && frame_right) {
if(left.data && right.data){
ImagePtr image_left(new Image);
ImagePtr image_right(new Image);
image_left->height = height;
image_left->width = width;
image_left->step = 3 * width;
image_left->encoding = image_encodings::RGB8;
image_left->header.stamp = capture_time;
image_left->header.seq = pair_id;
image_right->height = height;
image_right->width = width;
image_right->step = 3 * width;
image_right->encoding = image_encodings::RGB8;
image_right->header.stamp = capture_time;
image_right->header.seq = pair_id;
image_left->header.frame_id = frame;
image_right->header.frame_id = frame;
image_left->data.resize(image_left->step * image_left->height);
image_right->data.resize(image_right->step * image_right->height);
/*
if (rotate_left)
rotate(&image_left->data[0], frame_left, width * height);
else
memcpy(&image_left->data[0], frame_left, width * height * 3);
if (rotate_right)
rotate(&image_right->data[0], frame_right, width * height);
else
memcpy(&image_right->data[0], frame_right, width * height * 3);
*/
left_pub.publish(image_left);
right_pub.publish(image_right);
sendInfo(capture_time);
++pair_id;
}
frames_to_skip = skip_frames;
} else {
frames_to_skip--;
}
//if (frame_left) cam_left->release(left_idx);
//if (frame_right) cam_right->release(right_idx);
}
}
bool teleopFinishedServiceCallback(std_srvs::Empty::Request &req, std_srvs::Empty::Response &res) {
ROS_INFO("Will restart medicine task");
should_restart_medicine=true;
return true;
}
/*!
* \brief Initializes node to get parameters, subscribe and publish to topics.
*/
bool init()
{
// initialize member variables
isInitialized_ = false;
isDSAInitialized_ = false;
hasNewGoal_ = false;
// implementation of topics to publish
topicPub_JointState_ = nh_.advertise<sensor_msgs::JointState>("/joint_states", 1);
topicPub_ControllerState_ = nh_.advertise<pr2_controllers_msgs::JointTrajectoryControllerState>("state", 1);
topicPub_TactileSensor_ = nh_.advertise<schunk_sdh::TactileSensor>("tactile_data", 1);
// pointer to sdh
sdh_ = new SDH::cSDH(false, false, 0); //(_use_radians=false, bool _use_fahrenheit=false, int _debug_level=0)
// implementation of service servers
srvServer_Init_ = nh_.advertiseService("init", &SdhNode::srvCallback_Init, this);
srvServer_Stop_ = nh_.advertiseService("stop", &SdhNode::srvCallback_Stop, this);
srvServer_Recover_ = nh_.advertiseService("recover", &SdhNode::srvCallback_Recover, this);
srvServer_SetOperationMode_ = nh_.advertiseService("set_operation_mode", &SdhNode::srvCallback_SetOperationMode, this);
// getting hardware parameters from parameter server
nh_.param("sdhdevicetype", sdhdevicetype_, std::string("PCAN"));
nh_.param("sdhdevicestring", sdhdevicestring_, std::string("/dev/pcan0"));
nh_.param("sdhdevicenum", sdhdevicenum_, 0);
nh_.param("dsadevicestring", dsadevicestring_, std::string("/dev/ttyS0"));
nh_.param("dsadevicenum", dsadevicenum_, 0);
nh_.param("baudrate", baudrate_, 1000000);
nh_.param("timeout", timeout_, (double)0.04);
nh_.param("id_read", id_read_, 43);
nh_.param("id_write", id_write_, 42);
// get joint_names from parameter server
ROS_INFO("getting joint_names from parameter server");
XmlRpc::XmlRpcValue joint_names_param;
if (nh_.hasParam("joint_names"))
{
nh_.getParam("joint_names", joint_names_param);
}
else
{
ROS_ERROR("Parameter joint_names not set, shutting down node...");
nh_.shutdown();
return false;
}
DOF_ = joint_names_param.size();
joint_names_.resize(DOF_);
for (int i = 0; i<DOF_; i++ )
{
joint_names_[i] = (std::string)joint_names_param[i];
}
std::cout << "joint_names = " << joint_names_param << std::endl;
// define axes to send to sdh
axes_.resize(DOF_);
for(int i=0; i<DOF_; i++)
{
axes_[i] = i;
}
ROS_INFO("DOF = %d",DOF_);
state_.resize(axes_.size());
return true;
}
bool CollisionCheckingRos::collisionCheck(const std::vector <FrameWithId>& path,
const FrameWithId& actualPose, double interpolationStep, unsigned int numberOfSteps) {
bool collision = false;
if (path.empty())
return collision;
costmap_2d::Costmap2D costmapCopy;
std::vector <geometry_msgs::Point> orientedFootprint;
costmap->getOrientedFootprint(orientedFootprint);
costmap->clearRobotFootprint();
costmap->getCostmapCopy(costmapCopy);
base_local_planner::CostmapModel collisionChecker(costmapCopy);
std::vector <FrameWithId> prunedPath;
utilities::prunePath(path, actualPose, prunedPath);
LinearInterpolation interpolator;
std::vector <FrameWithId> interpolatedPath;
size_t counter = interpolator.interpolate(prunedPath, interpolatedPath, interpolationStep, numberOfSteps);
#ifdef DEBUG
Stopwatch stopwatch;
stopwatch.start();
#endif
size_t i;
for (i = 0; i < counter; ++i) {
const KDL::Frame& frame = interpolatedPath[i].getFrame();
double r, p, y;
frame.M.GetRPY(r, p, y);
geometry_msgs::PoseStamped pose;
conversions::frameToPoseStampedRos(frame, pose);
std::vector <geometry_msgs::Point> orientedFootprint;
costmap->getOrientedFootprint(frame.p.x(), frame.p.y(), 0, orientedFootprint);
double circumscribedRadius = costmap->getCircumscribedRadius();
double inscribedRadius = costmap->getInscribedRadius();
double c = collisionChecker.footprintCost(pose.pose.position,
orientedFootprint,
inscribedRadius,
circumscribedRadius);
if (c < 0) {
ROS_INFO("Collision at pose: %f, %f", pose.pose.position.x, pose.pose.position.y);
collision = true;
break;
}
}
#ifdef DEBUG_0
stopwatch.stop();
LOG("Collision checking using costmap2d_ros:");
LOG(" - collision: %d", collision);
LOG(" - step size: %f", interpolationStep);
LOG(" - iterations : %u", i);
LOG(" - duration : %f ms", stopwatch.getElapsedTime());
#endif
return collision;
}
/*!
* \brief Executes the service callback for init.
*
* Connects to the hardware and initialized it.
* \param req Service request
* \param res Service response
*/
bool srvCallback_Init(cob_srvs::Trigger::Request &req,
cob_srvs::Trigger::Response &res )
{
if (isInitialized_ == false)
{
//Init Hand connection
try
{
if(sdhdevicetype_.compare("RS232")==0)
{
sdh_->OpenRS232( sdhdevicenum_, 115200, 1, sdhdevicestring_.c_str());
ROS_INFO("Initialized RS232 for SDH");
isInitialized_ = true;
}
if(sdhdevicetype_.compare("PCAN")==0)
{
ROS_INFO("Starting initializing PEAKCAN");
sdh_->OpenCAN_PEAK(baudrate_, timeout_, id_read_, id_write_, sdhdevicestring_.c_str());
ROS_INFO("Initialized PEAK CAN for SDH");
isInitialized_ = true;
}
if(sdhdevicetype_.compare("ESD")==0)
{
ROS_INFO("Starting initializing ESD");
if(strcmp(sdhdevicestring_.c_str(), "/dev/can0") == 0)
{
ROS_INFO("Initializing ESD on device %s",sdhdevicestring_.c_str());
sdh_->OpenCAN_ESD(0, baudrate_, timeout_, id_read_, id_write_ );
}
else if(strcmp(sdhdevicestring_.c_str(), "/dev/can1") == 0)
{
ROS_INFO("Initializin ESD on device %s",sdhdevicestring_.c_str());
sdh_->OpenCAN_ESD(1, baudrate_, timeout_, id_read_, id_write_ );
}
else
{
ROS_ERROR("Currently only support for /dev/can0 and /dev/can1");
res.success.data = false;
res.error_message.data = "Currently only support for /dev/can0 and /dev/can1";
return true;
}
ROS_INFO("Initialized ESDCAN for SDH");
isInitialized_ = true;
}
}
catch (SDH::cSDHLibraryException* e)
{
ROS_ERROR("An exception was caught: %s", e->what());
res.success.data = false;
res.error_message.data = e->what();
delete e;
return true;
}
//Init tactile data
try
{
dsa_ = new SDH::cDSA(0, dsadevicenum_, dsadevicestring_.c_str());
//dsa_->SetFramerate( 0, true, false );
dsa_->SetFramerate( 1, true );
ROS_INFO("Initialized RS232 for DSA Tactile Sensors on device %s",dsadevicestring_.c_str());
// ROS_INFO("Set sensitivity to 1.0");
// for(int i=0; i<6; i++)
// dsa_->SetMatrixSensitivity(i, 1.0);
isDSAInitialized_ = true;
}
catch (SDH::cSDHLibraryException* e)
{
isDSAInitialized_ = false;
ROS_ERROR("An exception was caught: %s", e->what());
res.success.data = false;
res.error_message.data = e->what();
delete e;
return true;
}
}
else
{
ROS_WARN("...sdh already initialized...");
res.success.data = true;
res.error_message.data = "sdh already initialized";
}
res.success.data = true;
return true;
}
///\brief Opens joystick port, reads from port and publishes while node is active
int main(int argc, char **argv)
{
diagnostic_.add("Joystick Driver Status", this, &Joystick::diagnostics);
diagnostic_.setHardwareID("none");
// Parameters
ros::NodeHandle nh_param("~");
pub_ = nh_.advertise<sensor_msgs::Joy>("joy", 1);
nh_param.param<std::string>("dev", joy_dev_, "/dev/input/js0");
nh_param.param<double>("deadzone", deadzone_, 0.05);
nh_param.param<double>("autorepeat_rate", autorepeat_rate_, 0);
nh_param.param<double>("coalesce_interval", coalesce_interval_, 0.001);
nh_param.param<double>("max_period_with_no_events", max_period_with_no_events_, 0.5);
// Checks on parameters
if (autorepeat_rate_ > 1 / coalesce_interval_)
ROS_WARN("joy_node: autorepeat_rate (%f Hz) > 1/coalesce_interval (%f Hz) does not make sense. Timing behavior is not well defined.", autorepeat_rate_, 1/coalesce_interval_);
if (deadzone_ >= 1)
{
ROS_WARN("joy_node: deadzone greater than 1 was requested. The semantics of deadzone have changed. It is now related to the range [-1:1] instead of [-32767:32767]. For now I am dividing your deadzone by 32767, but this behavior is deprecated so you need to update your launch file.");
deadzone_ /= 32767;
}
if (deadzone_ > 0.9)
{
ROS_WARN("joy_node: deadzone (%f) greater than 0.9, setting it to 0.9", deadzone_);
deadzone_ = 0.9;
}
if (deadzone_ < 0)
{
ROS_WARN("joy_node: deadzone_ (%f) less than 0, setting to 0.", deadzone_);
deadzone_ = 0;
}
if (autorepeat_rate_ < 0)
{
ROS_WARN("joy_node: autorepeat_rate (%f) less than 0, setting to 0.", autorepeat_rate_);
autorepeat_rate_ = 0;
}
if (coalesce_interval_ < 0)
{
ROS_WARN("joy_node: coalesce_interval (%f) less than 0, setting to 0.", coalesce_interval_);
coalesce_interval_ = 0;
}
if ((max_period_with_no_events_ < 0) || (max_period_with_no_events_ > 1.0))
{
ROS_WARN("joy_node: max_period_with_no_events (%f) shoul be with [0.0, 1.0], setting to 0.", max_period_with_no_events_);
max_period_with_no_events_ = 0.0;
}
// Parameter conversions
double autorepeat_interval = 1 / autorepeat_rate_;
double scale = -1. / (1. - deadzone_) / 32767.;
double unscaled_deadzone = 32767. * deadzone_;
js_event event;
struct timeval tv;
fd_set set;
int joy_fd;
event_count_ = 0;
pub_count_ = 0;
lastDiagTime_ = ros::Time::now().toSec();
const ros::Duration max_duration_with_no_events(max_period_with_no_events_);
ros::Time last_event_time(0.0);
// Big while loop opens, publishes
while (nh_.ok())
{
open_ = false;
diagnostic_.force_update();
bool first_fault = true;
while (true)
{
ros::spinOnce();
if (!nh_.ok())
goto cleanup;
joy_fd = open(joy_dev_.c_str(), O_RDONLY);
if (joy_fd != -1)
{
// There seems to be a bug in the driver or something where the
// initial events that are to define the initial state of the
// joystick are not the values of the joystick when it was opened
// but rather the values of the joystick when it was last closed.
// Opening then closing and opening again is a hack to get more
// accurate initial state data.
close(joy_fd);
joy_fd = open(joy_dev_.c_str(), O_RDONLY);
}
if (joy_fd != -1)
break;
if (first_fault)
{
ROS_ERROR("Couldn't open joystick %s. Will retry every second.", joy_dev_.c_str());
first_fault = false;
}
sleep(1.0);
diagnostic_.update();
}
ROS_INFO("Opened joystick: %s. deadzone_: %f.", joy_dev_.c_str(), deadzone_);
open_ = true;
diagnostic_.force_update();
bool tv_set = false;
bool publication_pending = false;
tv.tv_sec = 1;
tv.tv_usec = 0;
sensor_msgs::Joy joy_msg; // Here because we want to reset it on device close.
while (nh_.ok())
{
ros::spinOnce();
bool publish_now = false;
bool publish_soon = false;
FD_ZERO(&set);
FD_SET(joy_fd, &set);
//ROS_INFO("Select...");
int select_out = select(joy_fd+1, &set, NULL, NULL, &tv);
//ROS_INFO("Tick...");
if (select_out == -1)
{
tv.tv_sec = 0;
tv.tv_usec = 0;
//ROS_INFO("Select returned negative. %i", ros::isShuttingDown());
continue;
// break; // Joystick is probably closed. Not sure if this case is useful.
}
if (FD_ISSET(joy_fd, &set))
{
if (read(joy_fd, &event, sizeof(js_event)) == -1 && errno != EAGAIN)
break; // Joystick is probably closed. Definitely occurs.
//ROS_INFO("Read data...");
joy_msg.header.stamp = ros::Time().now();
event_count_++;
switch(event.type)
{
case JS_EVENT_BUTTON:
case JS_EVENT_BUTTON | JS_EVENT_INIT:
if(event.number >= joy_msg.buttons.size())
{
int old_size = joy_msg.buttons.size();
joy_msg.buttons.resize(event.number+1);
for(unsigned int i=old_size;i<joy_msg.buttons.size();i++)
joy_msg.buttons[i] = 0.0;
}
joy_msg.buttons[event.number] = (event.value ? 1 : 0);
// For initial events, wait a bit before sending to try to catch
// all the initial events.
if (!(event.type & JS_EVENT_INIT))
{
publish_now = true;
last_event_time = ros::Time::now();
}
else
publish_soon = true;
break;
case JS_EVENT_AXIS:
case JS_EVENT_AXIS | JS_EVENT_INIT:
if(event.number >= joy_msg.axes.size())
{
int old_size = joy_msg.axes.size();
joy_msg.axes.resize(event.number+1);
for(unsigned int i=old_size;i<joy_msg.axes.size();i++)
joy_msg.axes[i] = 0.0;
}
if (!(event.type & JS_EVENT_INIT)) // Init event.value is wrong.
{
double val = event.value;
// Allows deadzone to be "smooth"
if (val > unscaled_deadzone)
val -= unscaled_deadzone;
else if (val < -unscaled_deadzone)
val += unscaled_deadzone;
else
val = 0;
joy_msg.axes[event.number] = val * scale;
}
// Will wait a bit before sending to try to combine events.
publish_soon = true;
last_event_time = ros::Time::now();
break;
default:
ROS_WARN("joy_node: Unknown event type. Please file a ticket. time=%u, value=%d, type=%Xh, number=%d", event.time, event.value, event.type, event.number);
break;
}
}
else if (tv_set) // Assume that the timer has expired.
publish_now = true;
if (publish_now)
{
// Assume that all the JS_EVENT_INIT messages have arrived already.
// This should be the case as the kernel sends them along as soon as
// the device opens.
//ROS_INFO("Publish...");
//pub_.publish(joy_msg);
publish_now = false;
tv_set = false;
publication_pending = false;
publish_soon = false;
//pub_count_++;
if ((ros::Time::now() - last_event_time) < max_duration_with_no_events)
{
pub_.publish(joy_msg);
pub_count_++;
}
}
// If an axis event occurred, start a timer to combine with other
// events.
if (!publication_pending && publish_soon)
{
tv.tv_sec = trunc(coalesce_interval_);
tv.tv_usec = (coalesce_interval_ - tv.tv_sec) * 1e6;
publication_pending = true;
tv_set = true;
//ROS_INFO("Pub pending...");
}
// If nothing is going on, start a timer to do autorepeat.
if (!tv_set && autorepeat_rate_ > 0)
{
tv.tv_sec = trunc(autorepeat_interval);
tv.tv_usec = (autorepeat_interval - tv.tv_sec) * 1e6;
tv_set = true;
//ROS_INFO("Autorepeat pending... %i %i", tv.tv_sec, tv.tv_usec);
}
if (!tv_set)
{
tv.tv_sec = 1;
tv.tv_usec = 0;
}
diagnostic_.update();
} // End of joystick open loop.
close(joy_fd);
ros::spinOnce();
if (nh_.ok())
ROS_ERROR("Connection to joystick device lost unexpectedly. Will reopen.");
}
cleanup:
ROS_INFO("joy_node shut down.");
return 0;
}
LaserScanMatcher::LaserScanMatcher(ros::NodeHandle nh, ros::NodeHandle nh_private):
nh_(nh),
nh_private_(nh_private),
initialized_(false),
received_imu_(false),
received_odom_(false),
received_vel_(false)
{
ROS_INFO("Starting LaserScanMatcher");
// **** init parameters
initParams();
// **** state variables
f2b_.setIdentity();
f2b_kf_.setIdentity();
input_.laser[0] = 0.0;
input_.laser[1] = 0.0;
input_.laser[2] = 0.0;
// **** publishers
if (publish_pose_)
{
pose_publisher_ = nh_.advertise<geometry_msgs::Pose2D>(
"pose2D", 5);
}
if (publish_pose_stamped_)
{
pose_stamped_publisher_ = nh_.advertise<geometry_msgs::PoseStamped>(
"pose_stamped", 5);
}
// *** subscribers
if (use_cloud_input_)
{
cloud_subscriber_ = nh_.subscribe(
"cloud", 1, &LaserScanMatcher::cloudCallback, this);
}
else
{
scan_subscriber_ = nh_.subscribe(
"scan", 1, &LaserScanMatcher::scanCallback, this);
}
if (use_imu_)
{
imu_subscriber_ = nh_.subscribe(
"imu/data", 1, &LaserScanMatcher::imuCallback, this);
}
if (use_odom_)
{
odom_subscriber_ = nh_.subscribe(
"odom", 1, &LaserScanMatcher::odomCallback, this);
}
if (use_vel_)
{
if (stamped_vel_)
vel_subscriber_ = nh_.subscribe(
"vel", 1, &LaserScanMatcher::velStmpCallback, this);
else
vel_subscriber_ = nh_.subscribe(
"vel", 1, &LaserScanMatcher::velCallback, this);
}
}