int main(int argc, char **argv)
{
	ros::init(argc, argv, "rps_robot_commander");
	ros::NodeHandle n;

	//~ ros::Subscriber	rps_map_subscriber = n.subscribe("rps_map_data", 1, set_RPS_MAP);
	ros::ServiceServer server_robot_command = n.advertiseService("rps_robot_command", start_robot_commander);

	commander_to_get_robots_info = n.serviceClient<tms_msg_db::tmsdb_get_robots_info>("tmsdb_get_robots_info");
#ifdef SEND_COMMAND
	client_smartpal5_control = n.serviceClient<tms_msg_rc::smartpal_control>("sp5_control");
#endif
	client_smartpal5_speak = n.serviceClient<tms_msg_rc::smartpal_speak>("sp5_speak");
	
	////////////////////////////
#ifdef USE_TMS_DB
	srv_r.request.robots_id = 2;
	if(commander_to_get_robots_info.call(srv_r))
		ROS_INFO("Success robots_x = %lf, y = %lf, theta = %lf", srv_r.response.robots_x,srv_r.response.robots_y,srv_r.response.robots_theta);
	else{
		ROS_ERROR("Failed to call service get_robots_info\n");
		return 0;
	}
	srv_smartpal_control.request.unit = UNIT_VEHICLE;
	srv_smartpal_control.request.cmd  = CMD_setPose;
	srv_smartpal_control.request.arg.resize(3);
	srv_smartpal_control.request.arg[0] = srv_r.response.robots_x;
	srv_smartpal_control.request.arg[1] = srv_r.response.robots_y;
	srv_smartpal_control.request.arg[2] = srv_r.response.robots_theta;
#else
	srv_smartpal_control.request.unit = UNIT_VEHICLE;
	srv_smartpal_control.request.cmd  = CMD_setPose;
	srv_smartpal_control.request.arg.resize(3);
	srv_smartpal_control.request.arg[0] = 1000;
	srv_smartpal_control.request.arg[1] = 1000;
	srv_smartpal_control.request.arg[2] = 0.0;
#endif
	
#ifdef SEND_COMMAND
	if(client_smartpal5_control.call(srv_smartpal_control)){
		ROS_INFO("result: %d", srv_smartpal_control.response.result);
	}	
	else{
		ROS_ERROR("Failed to call service sp5_control");
		return 0;
	}
#endif
	////////////////////////////
	
	ros::spin();

	return 0;
}
void emergencyStopHandler(std_msgs::Bool msg)
{
	if (msg.data)
	{
		pending_bricks.clear();
		robot_client.call(cmdResetMotion,cmdRes);
		robot_state = emergency_stop;
	}
	else
	{
		robot_client.call(cmdOpenGripper,cmdRes);
		robot_state = idle;
	}
}
void joint_state_cb(const sensor_msgs::JointStateConstPtr& js){

    if (js->position.size() > 4){ //Message from the base or the arm
        js_cur = *js;
        
        last_status = sbs;
		if(!checkIfSafe())
			sbs.status = sbs.PAUSED;
		else
			sbs.status = sbs.RUNNING;
			
		sb_req.status = sbs;
		sb_req.requester = "mico_safety_node";
		
		if(sbs.status != last_status.status){
			if(stopbase_client.call(sb_req,sb_resp)){
				ROS_DEBUG("Made stop_base call");
			}
			else{
				ROS_ERROR("Stop base service call failed!");
				ros::shutdown();
			}	
		}
    }

};
Beispiel #4
0
/*
 * Check the busy flag
 *
 * Poll for the busy flag returns true if busy, false if not
 */
bool isADCBusy(ADC adc)
{
    //Define and fill in a service request
    hardware_interface::ReadI2CRegister read;
    read.request.addr = adc;
    read.request.reg = ADC_BUSY;
    read.request.size = 1;

    //Send request
    if(read_register_svr.call(read))
    {
        //Sussecful read
        if(read.response.data[0])
        {
            //Still busy
            return true;
        }
        else
        {
            //Not busy!
            return false;
        }
    }
    else
    {
        //Unsussesful read!!!
        //Do error handeling
    }

    return false;
}
Beispiel #5
0
void moveToJointState(const float* js){
	
	/*moveit::planning_interface::MoveGroup group("arm");
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;   
    group.setPlanningTime(5.0); //10 second maximum for collision computation*/
	
	
	
	moveit_utils::MicoMoveitJointPose::Request req;
	moveit_utils::MicoMoveitJointPose::Response res;
	
	for(int i = 0; i < 6; i++){
        switch(i) {
            case 0  :    req.target.joint1 = js[i]; break;
            case 1  :    req.target.joint2 =  js[i]; break;
            case 2  :    req.target.joint3 =  js[i]; break;
            case 3  :    req.target.joint4 =  js[i]; break;
            case 4  :    req.target.joint5 =  js[i]; break;
            case 5  :    req.target.joint6 =  js[i]; break;
        }
	//ROS_INFO("Requested angle: %f", q_vals.at(i));
    }
	
	if(client_joint_command.call(req, res)){
 		ROS_INFO("Call successful. Response:");
 		ROS_INFO_STREAM(res);
 	} else {
 		ROS_ERROR("Call failed. Terminating.");
 		//ros::shutdown();
 	}
}
  void jointsCall()
  {
    //ここをどうするか?
    for(int i = 0; i < okao_id.size(); ++i)
      {

	humans_msgs::HumanImgSrv hs;
	//hs.request.rule = rule;
       	
	hs.request.src.max_okao_id = okao_id[i];
	
	//okao_client::OkaoStack okao_img;
	//okao_img.request.person.okao_id = okao_id;
	
	if( srv.call( hs ) )
	  {	
	    //cout << "okao_id:" << okao_id[i] << ", x,y =" 
	    //	 << hs.response.dst.p.x << ","<< hs.response.dst.p.y <<  endl ;
	    markerPublisher( hs.response.dst, hs.response.img );
	  }
	else
	  {
	    cout << "not found!"<<endl;
	  }
      }


  }
Beispiel #7
0
/* processData is the main decision function that
 * will publish commands to the motor controller
 * node and possibly ask for arduino data.
 */
void processData() {

   //geometry_msgs::Twist command;
   beagle_pkg::status status;
   if(ard_client.call(ard_data)) {

      ROS_INFO("lidar data was %f, arduino reading was %f\n", scan_data.range_min, ard_data.response.distance);

      if(ard_data.response.distance > 5 && scan_data.range_min > 5) {
         status.command = 0;
      }
      else if(ard_data.response.distance <= 0 && scan_data.range_min <= 0) {
         status.command = 0;
      }
      else if(ard_data.response.distance > scan_data.range_min) {
         status.command = 1;
      }
      else {
         status.command = 2;
      }
      
      /* TODO: look at global data (scan_data, gps_data, ard_data)
       * and make decision of what to send to the motor node.
       */
      vel_pub.publish(status);
   }
   else {
    ROS_ERROR("Failed to call ard service");
   }
}
Beispiel #8
0
//=============================================================
//名前:TurnScan
//説明:首振りの方向折り返し
//=============================================================
void ThermalScanning::TurnScan(void)
{
	//std_msgs::Float64 tilt_controller_command_msg; 

	if(cw == -1)	// 左回りの場合
	{
		start_angle = MIN_YAW_ANGLE_CAM;
		end_angle   = MAX_YAW_ANGLE_CAM;
		pm = 1;
	}else if(cw == 1){	// 右回りの場合
		start_angle = MAX_YAW_ANGLE_CAM;
		end_angle   = MIN_YAW_ANGLE_CAM;
		pm = -1;
	}

	// ダイナミクセルのスピードの設定
	if(Scan_state == 0 || Scan_state == 5 || Scan_state == 6)	// 通常探索
	{ 
		srv.request.speed = MOTOR_SPEED;
	}
	else if(Scan_state == 1)	// 強化探索
	{
		srv.request.speed = MOTOR_SPEED2;
	}	
	dyna_client.call(srv);
		
	// ダイナミクセルの角度を初期位置に移動する
	// TODO ダイナミクセルの角度をDyna_angleに変更する処理を入れる
	Dyna_angle = start_angle;

	tilt_controller_command_msg.data = Dyna_angle;
}
int main(int argc, char **argv){
    ros::init(argc, argv, "run_pose_estimation_for_scene_screenshot");

    ros::NodeHandle nodeHandle = ros::NodeHandle("~");
    global = nodeHandle.serviceClient<MsgT>("/inSceneDetector/detect");

    if (!once){


        //get screen shot pose
        ROS_INFO("Subscribing to detection service for screen shot pose...");
        ros::service::waitForService("/inSceneDetector/detect");
        MsgT msgglobal01;
        msgglobal01.request.scenario = "detect_screen_shot";

            if(global.call(msgglobal01)) {
			//printResults(msgglobal01);
                    pcl::console::print_value("Screenshot detected!\n");
            } else pcl::console::print_error("Screenshot not detected!\n");

	once = true;
    }
    ros::spinOnce();

return 0;
}
Beispiel #10
0
std::vector<Brick> getBricks()
{
    std::vector<Brick> retVec;
    rc_vision::getBricks obj;

    if(!_serviceGetBricks.call(obj))
    {
        printConsole("Failed to call the 'serviceGetBricks'");
    }
    else
    {
        for(unsigned int i=0; i<obj.response.size.size(); i++)
        {
            Brick brick;
            brick.color = obj.response.color[i];
            brick.posX = obj.response.posX[i];
            brick.posY = obj.response.posY[i];
            brick.size = obj.response.size[i];
            brick.theta = obj.response.theta[i];
            retVec.push_back(brick);
        }
    }

    return retVec;
}
Beispiel #11
0
void anyBrickCallback(std_msgs::Bool msg)
{
    // Fetch robot position
    kuka_rsi::getConfiguration getQObj;

    // Call service
    if(!_serviceGetConf.call(getQObj))
       printConsole("Failed to call the 'serviceKukaGetConfiguration'");

    // Get information
    bool same = true;
    for(int i=0; i<6; i++)
    {
        if(fabs(getQObj.response.q[i]*DEGREETORAD - _qIdle[i]) > 0.1)
        {
            same = false;
            break;
        }
    }

    _anyBricksMutex.lock();
    _anyBricks = msg.data;
    _anyBricksMutex.unlock();

    _qMutex.lock();
    _positionQIdle = same;
    _qMutex.unlock();
}
Beispiel #12
0
void changeDirConveyerBelt(bool dir) // true = reverse
{
    rc_plc::ChangeDirection obj;
    obj.request.direction = dir;
    if(!_serviceChangeDir.call(obj))
        printConsole("Failed to call the 'serviceChangeDirConveyer'");
}
Beispiel #13
0
void startConveyerBelt(bool dir = false)
{
    rc_plc::StartConv obj;
    obj.request.direction = dir;
    if(!_serviceStart.call(obj))
        printConsole("Failed to call the 'serviceStartConveyer'");
}
/**
 * @brief Callback functions for datalink routing, channel 1 
 *        
 */
void sub1_callback(const uav_control::DFrame msg)
{
  // message from GCS, to be relayed to quadcopter
  if(msg.target_id != SYSID){
    uav_control::datalink_send ss;
    ss.request.source_id = msg.source_id;
    ss.request.target_id = msg.target_id;
    ss.request.route = msg.route;
    ss.request.len = msg.len;
    int i;
    for(i=0; i<msg.len; i++)
      ss.request.payload[i] = msg.payload[i];

    if(client_sub1.call(ss)){
      switch(ss.response.err){
        case 0: //ROS_INFO("successful");
                break;
        case 1: ROS_ERROR("relay error: invalid channel");
                break;
        case 2: ROS_ERROR("relay error: channel not initialized");
                break;
        case 3: ROS_ERROR("relay error: sending failed");
                break;
        default: ROS_ERROR("relay error: unknown failure");
                break;
      }
    }
  }

}
bool plotSummary(ros::ServiceClient& sum_client)
{
    PlanningSummary planning_sum;
    if (!sum_client.call(planning_sum))
    {
        return false;
    }

    unsigned int cols = planning_sum.response.score_labels.size();
    for (unsigned int i = 0; i < planning_sum.response.score_labels.size(); i++)
    {
        cout << planning_sum.response.score_labels[i] << "\t";
    }
    cout << endl;

    for (unsigned int i = 0; i < planning_sum.response.score_values.size(); i++)
    {
        if (i % cols == 0)
        {
            cout << i / cols << ": ";
        }
        cout << planning_sum.response.score_values[i];
        cout << "\t";
        if (i % cols == cols - 1)
        {
            cout << endl;
        }
    }

    return true;
}
void TrajectoryMapWriter::draw(MapWriterInterface *interface)
{
    if(!initialized_) return;

    hector_nav_msgs::GetRobotTrajectory srv_path;
    if (!service_client_.call(srv_path)) {
      ROS_ERROR_NAMED(name_, "Cannot draw trajectory, service %s failed", service_client_.getService().c_str());
      return;
    }

    std::vector<geometry_msgs::PoseStamped>& traj_vector (srv_path.response.trajectory.poses);

    size_t size = traj_vector.size();

    std::vector<Eigen::Vector2f> pointVec;
    pointVec.resize(size);

    for (size_t i = 0; i < size; ++i){
      const geometry_msgs::PoseStamped& pose (traj_vector[i]);

      pointVec[i] = Eigen::Vector2f(pose.pose.position.x, pose.pose.position.y);
    }

    if (size > 0){
      //Eigen::Vector3f startVec(pose_vector[0].x,pose_vector[0].y,pose_vector[0].z);
      Eigen::Vector3f startVec(pointVec[0].x(),pointVec[0].y(),0.0f);
      interface->drawPath(startVec, pointVec);
    }
}
Beispiel #17
0
  void testIKServiceCall()
  {
    // define goal pose for right hand end effector
    Eigen::Affine3d goal_pose = Eigen::Affine3d::Identity();
    goal_pose *= Eigen::AngleAxisd(3.141593, Eigen::Vector3d::UnitY());
    goal_pose.translation()[0] = 0.6;
    goal_pose.translation()[1] = -0.5;
    goal_pose.translation()[2] = 0.1;
    visual_tools_->publishAxisLabeled(goal_pose, "goal_pose");

    // call ik service
    geometry_msgs::PoseStamped tf2_msg;
    tf2_msg.header.stamp = ros::Time::now();
    tf::poseEigenToMsg(goal_pose, tf2_msg.pose);

    ik_srv_.request.pose_stamp.push_back(tf2_msg);


    // TODO: not the right way to call this? compile error on this section...
    if (ik_client_right_.call(ik_srv_))
    {
      ROS_DEBUG_STREAM_NAMED("testIKServiceCall","ik service called successfully.");
    }
    else
    {
      ROS_WARN_STREAM_NAMED("testIKServiceCall","Failed to call ik service...");
    }

  }
		bool sendAndMonitorTask()
		{
			ArmRequest req;
			ArmResponse res;
			uint32_t srv_call_counter = 0;
			bool succeeded = false;
			std::vector<TaskDetails> recovery_tasks;

			req.tasks_codes = tasks_codes_;
			while((srv_call_counter++ < attempts_) && !succeeded)
			{
				ROS_INFO_STREAM(ros::this_node::getName()<<": Task '"<<name_<<"' attempt "<<srv_call_counter);
				succeeded = arm_client_.call(req,res) && (bool)res.completed;
				ROS_INFO_STREAM(ros::this_node::getName()<<": Task '"<<name_<<"' state "
						<<(succeeded ?" succeeded":" failed"));
			}
			error_code_ = res.error_code;

			// on failure execute recovery task serially
			if(!succeeded && getRecoveryTasks(recovery_tasks))
			{
				std::vector<TaskDetails>::iterator i;
				for(i = recovery_tasks.begin(); i != recovery_tasks.end(); i++)
				{
					if(!i->sendAndMonitorTask() )
					{
						// checking termination error, and exiting
						recovery_task_termination_error_ = i->terminationErrorReceived();
						break;
					}
				}
			}
			completed_ = succeeded;
			return completed_;
		}
Beispiel #19
0
  unsigned int nrControllers()
  {

    pr2_mechanism_msgs::ListControllers srv_msg;
    if (!list_srv_.call(srv_msg)) return 0;;
    return srv_msg.response.controllers.size();
  }
void heartMonitor(ros::NodeHandle n, ros::ServiceClient client, estop_control::estopSignal srv)
{
    time(&now);  // get current time; same as: now = time(NULL)

    if(heartbeat == true)
    {
        heartbeat = false;
        ROS_INFO("beating");
        previousTime = now;
    }

    if(difftime(now, previousTime) > 1)  // 1 second delay
    {
        ROS_INFO("heart stopped"); //estop

//        ros::ServiceClient client = n.serviceClient<estop_control::estopSignal>("estop_control");
//        estop_control::estopSignal srv;
        srv.request.message = 1;
        if (client.call(srv))
        {
          ROS_INFO("Recieved handshake: %d", (bool)srv.response.handshake);
        }
        else
        {
          ROS_ERROR("Failed to call service estop_control");
        }
        //client.shutdown();
    }
}
void callback(const nav_msgs::Odometry::ConstPtr &odom)
{
    if(isnan(last_point.x) && isnan(last_point.y) && isnan(last_point.z)) {
        last_point = odom->pose.pose.position;
        return;
    }

    double distance = sqrt(pow(odom->pose.pose.position.x-last_point.x,2)+pow(odom->pose.pose.position.y-last_point.y,2));
    total_distance += distance;

    std_msgs::Float32 pub_dist;
    pub_dist.data = (float) total_distance;
    mileage_pub.publish(pub_dist);

    last_point = odom->pose.pose.position;

    if(save % save_interval == 0) {
        ros::param::set("/saved_mileage",total_distance);
        srv.request.param = "saved_mileage";
        if (client.call(srv))
            ROS_DEBUG("Save mileage: success");
        else
            ROS_WARN("Save mileage: failed");
        save = 0;
    }
    save++;
}
bool callService::callTheService()
{
  if(client_.call(srv_)){
    double x,y,z,qx,qy,qz,qw;
    x = srv_.response.final_pose.position.x;
    y = srv_.response.final_pose.position.y;
    z = srv_.response.final_pose.position.z;
    qx = srv_.response.final_pose.orientation.x;
    qy = srv_.response.final_pose.orientation.y;
    qz = srv_.response.final_pose.orientation.z;
    qw = srv_.response.final_pose.orientation.w;
    ROS_INFO("Pose: tx= %5.4lf  %5.4lf  %5.4lf quat= %5.3lf  %5.3lf  %5.3lf %5.3lf, cost= %5.3lf",
	     srv_.response.final_pose.position.x,
	     srv_.response.final_pose.position.y,
	     srv_.response.final_pose.position.z,
	     srv_.response.final_pose.orientation.x,
	     srv_.response.final_pose.orientation.y,
	     srv_.response.final_pose.orientation.z,
	     srv_.response.final_pose.orientation.w,
	     srv_.response.final_cost_per_observation);
    tf::Transform camera_to_target;
    tf::Quaternion quat(qx,qy,qz,qw);
    camera_to_target.setOrigin(tf::Vector3(x,y,z));
    camera_to_target.setRotation(quat);
    tf::StampedTransform stf(camera_to_target, ros::Time::now(), optical_frame_.c_str(), "target_frame");
    tf_broadcaster_.sendTransform(stf);
    return(true);
  }
  ROS_ERROR("Target Location Failure");
  return(false);

}
Beispiel #23
0
    virtual void Update( ) {
/*
        common::Time sim_time = world->GetSimTime();

        Real time = (sim_time - start_time).Double();
        Real position = this->joints[0]->GetAngle( 0 ).Radian( );
        Real velocity = this->joints[0]->GetVelocity( 0 );
        Real torque = control( time, position, velocity );

        this->joints[0]->SetForce( 0, torque );

        last_time = sim_time;
*/
        gazebo::common::Time sim_time = world->GetSimTime();
        pendulum::rpc_command state;

        state.request.time = (sim_time - start_time).Double();
        state.request.position = this->joints[0]->GetAngle( 0 ).Radian( );
        state.request.velocity = this->joints[0]->GetVelocity( 0 );

	if( client.call(state) ) {
            Real torque = state.response.torque;	
            this->joints[0]->SetForce( 0, torque );
        }

        last_time = sim_time;
    }
rvMCUReport getReport()
{
  rvMCUReport rv;
  if (reportClient.call(reportSrv))
  {
    rv.length = reportSrv.response.length;
    rv.lDirection = reportSrv.response.lDirection;
    rv.lNum = reportSrv.response.lNum;
    rv.rDirection = reportSrv.response.rDirection;
    rv.rNum = reportSrv.response.rNum;
    rv.rearDirection = reportSrv.response.rearDirection;
    rv.rearNum = reportSrv.response.rearNum;
    rv.headPosition = reportSrv.response.headPosition;
    rv.isLedOn = reportSrv.response.isLedOn;
    rv.isIrOn = reportSrv.response.isIrOn;
    rv.isDetectedBarrier = reportSrv.response.isDetectedBarrier;
    ROS_INFO("MCU Report:\nlength=%d", rv.length);
    ROS_INFO("Left direction:num=%d:%d", rv.lDirection, rv.lNum);
    ROS_INFO("Right direction:num=%d:%d", rv.rDirection, rv.rNum);
    ROS_INFO("Rear direction:num=%d:%d", rv.rearDirection, rv.rearNum);
    ROS_INFO("headPosition=%d", rv.headPosition);
    ROS_INFO("isLedOn=%d,isIrOn=%d,isDetectedBarrier=%d", rv.isLedOn, rv.isIrOn, rv.isDetectedBarrier);
  }
  else
  {
    ROS_ERROR("Failed to call service rovioReport");
  }

  return rv;
}
 void send_reset() {
     hdpc_com::ChangeStateMachine change_state;
     change_state.request.event = EVENT_RESET;
     if (!state_machine_client.call(change_state)) {
         ROS_WARN("Change state machine: reset request failed");
     }
 }
Beispiel #26
0
/*
createSimulatePlaneCallback
This is the callback used to handle any users wishing to create a new plane.  This could be used for setting
up a course as well.
*/
bool createSimulatedPlaneCallback(AU_UAV_ROS::CreateSimulatedPlane::Request &req, AU_UAV_ROS::CreateSimulatedPlane::Response &res)
{
	//create a service to get a plane ID to use
	AU_UAV_ROS::RequestPlaneID srv;
	srv.request.requestedID = req.requestedID;
	
	//check to make sure the client call worked (regardless of return values from service)
	if(requestPlaneIDClient.call(srv))
	{
		res.planeID = srv.response.planeID;
		
		if(srv.response.planeID == -1)
		{
			ROS_ERROR("Couldn't create plane with ID %d", req.requestedID);
			return false;
		}

		
		//create and add our plane to the list of simulated planes
		//AU_UAV_ROS::SimulatedPlane newPlane(srv.response.planeID, req);
		simPlaneMap[srv.response.planeID] = AU_UAV_ROS::SimulatedPlane(srv.response.planeID, req);
		
		//plane created successfully
		return true;
	}
	else
	{
		//if this happens, chances are the coordinator isn't running
		ROS_ERROR("Did not receive an ID from coordinator");
		return false;
	}
}
void ubicacionCallback(const vrep_common::ObjectGroupData msgUbicacionPatas)
{
	for(int k=1; k<Npatas+1; k++){
        ubicacionRobot.coordenadaPata_x[k-1]=msgUbicacionPatas.floatData.data[0+k*Npatas/2];
        ubicacionRobot.coordenadaPata_y[k-1]=msgUbicacionPatas.floatData.data[1+k*Npatas/2];
        ubicacionRobot.coordenadaPata_z[k-1]=msgUbicacionPatas.floatData.data[2+k*Npatas/2];

    //-- Transformacion de trayectoria a Sistema de Pata
        srv_Trans_MundoPata.request.modo=Mundo_Pata;
        srv_Trans_MundoPata.request.Npata=k-1;
        srv_Trans_MundoPata.request.x_S0=ubicacionRobot.coordenadaPata_x[k-1];
        srv_Trans_MundoPata.request.y_S0=ubicacionRobot.coordenadaPata_y[k-1];
        srv_Trans_MundoPata.request.z_S0=ubicacionRobot.coordenadaPata_z[k-1];
        srv_Trans_MundoPata.request.x_UbicacionRob=ubicacionRobot.coordenadaCuerpo_x;
        srv_Trans_MundoPata.request.y_UbicacionRob=ubicacionRobot.coordenadaCuerpo_y;
        srv_Trans_MundoPata.request.theta_Rob=ubicacionRobot.orientacionCuerpo_yaw;
        if (client_Trans_MundoPata.call(srv_Trans_MundoPata))
        {   ubicacionRobot.coordenadaPataSistemaPata_x[k-1] = srv_Trans_MundoPata.response.x_Pata;
            ubicacionRobot.coordenadaPataSistemaPata_y[k-1] = srv_Trans_MundoPata.response.y_Pata;
            ubicacionRobot.coordenadaPataSistemaPata_z[k-1] = srv_Trans_MundoPata.response.z_Pata;
    //                ROS_INFO("Servicio motores: q1=%.3f; q2=%.3f; q3=%.3f\n", _q1, _q2, _q3);
        } else {
            ROS_ERROR("Nodo6:[%d] servicio de Trans_MundoPata no funciona\n",k-1);
    //        return;
        }

//    ROS_INFO("Nodo6: pata[%d], x=%.3f, y=%.3f",k-1,ubicacionRobot.coordenadaPata_x[k-1],ubicacionRobot.coordenadaPata_y[k-1]);
    }
    infoPatas = true;
}
/*
    Function to place the currently grasped object on the object named <container_name>
*/
void place(std::string container_name)
{

    world_model_srv.request.base_frame = "gripperbot_base";
    world_model_srv.request.target_frame = container_name;

    if(world_model_client.call(world_model_srv))
    {
        if(world_model_srv.response.success)
        {
            if(world_model_srv.response.pose.position.x < 0)
                return;
            if(world_model_srv.response.pose.position.z < 0)
                world_model_srv.response.pose.position.z = -world_model_srv.response.pose.position.z;

            movegripperbot(world_model_srv.response.pose.position.x, world_model_srv.response.pose.position.y, world_model_srv.response.pose.position.z+0.1);
            movegripperbot(world_model_srv.response.pose.position.x, world_model_srv.response.pose.position.y, world_model_srv.response.pose.position.z);
            opengripper();
            movegripperbot(world_model_srv.response.pose.position.x, world_model_srv.response.pose.position.y, world_model_srv.response.pose.position.z+0.1);
        }
        else
        {
            ROS_ERROR("Transform between object and gripperbot_base doesnt exist");
        }
    }
    else
    {
        ROS_ERROR("Failed to call service /world_model/get_transform");
    }

}
void moveMouse(ros::ServiceClient& client, const cv::Point2f& goal)
{
    agario_mouse::Mouse srv;
    srv.request.x = (int)goal.x + MOUSE_CALIBRATE_X;
    srv.request.y = (int)goal.y + MOUSE_CALIBRATE_Y;
    client.call(srv);
}
bool rotate_cb(mantis_data_collection::dataCollect::Request &req, mantis_data_collection::dataCollect::Response &res)
{
std_msgs::UInt16 command;
command.data=0;
mantis_data_collection::process_cloud srv;

srv.request.objectName = req.objectName;
//following line added by CG along with following // comments to make useful for one angle
command.data = req.delta;

//while(command.data<360)
//{
//ros::Rate loop_rate(.1);

srv.request.in_cloud = cloud_to_process;
srv.request.angle = command.data;
pan_client.call(srv);
srv.response.result = 1;

pan_pub.publish(command);
//std::cout << "Angle " << srv.request.angle << "\n";
//ros::spinOnce();
//loop_rate.sleep();
//command.data += req.delta;
//}

res.result = 1;
std::cout << "Status: " << srv.response.result << "\n";
return 1;
}