void StepShm(int cntr)
{   

    if (!status.calibrated)
    {
	printf("ZLift is not calibrated.  Please calibrate and run again.  Exiting.\n");
	endme(1);
    }
  
    SetTimestamp(GetTimestamp()); //Pass back timestamp as a heartbeat

    joint_state_g.header.stamp = ros::Time::now();  
    
    joint_state_g.position[0] = status.position;
    joint_state_g.velocity[0] = status.velocity;
    joint_state_g.effort[0] = status.effort;

    zlift_publisher_g.publish(joint_state_g);
    
     /*if (cntr % 100 == 0)
      {	
	if (1)
	{
	  printf("********************************\n");
	  printf("timestamp: %ld\n", status.timestamp);	  
	  {	    	    
	    printf("------------------------------\n");
	    printf("position: %f\n", status.position);
	    printf("velocity: %f\n", status.velocity);
	    printf("effort: %f\n", status.effort);	    
	     printf("------------------------------\n");
	    printf("\n");
	  }
	}
      }*/
    
    /*cmd.position = 600;
    cmd.velocity = 2000;
    cmd.stiffness = 1.0;
    cmd.control_mode = JOINT_MODE_ROS_THETA_GC;
    cmd.smoothing_mode = SMOOTHING_MODE_SLEW;*/
   
}
Пример #2
0
void exit_on_error()
{
	endme(0);
}
void StepShm(int cntr)
{
    SetTimestamp(GetTimestamp()); //Pass back timestamp as a heartbeat

    if (!status.calibrated)
    {
        printf("Omnibase is not calibrated.  Please calibrate and run again.  Exiting.\n");
        endme(1);
    }

    odom_g.header.stamp = ros::Time::now();


    // get from status
    double x = status.x;
    double y = status.y;
    double th = status.yaw;

    double vx = status.x_dot;
    double vy = status.y_dot;
    double vth = status.yaw_dot;
    //ROS_INFO("[STATUS] x,y,th:[%f,%f,%f]",x,y,th);
    // get from status

    //since all odometry is 6DOF we'll need a quaternion created from yaw
    geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

    //first, we'll publish the transform over tf
    geometry_msgs::TransformStamped odom_trans;
    odom_trans.header.stamp = ros::Time::now();
    odom_trans.header.frame_id = "odom";
    odom_trans.child_frame_id = "base_link";

    odom_trans.transform.translation.x = x;
    odom_trans.transform.translation.y = y;
    odom_trans.transform.translation.z = 0.0;
    odom_trans.transform.rotation = odom_quat;

    //send the transform
    //odom_broadcaster.sendTransform(odom_trans);

    odom_g.header.frame_id = "odom";

    //set the position
    odom_g.pose.pose.position.x = x;
    odom_g.pose.pose.position.y = y;
    odom_g.pose.pose.position.z = 0.0;
    odom_g.pose.pose.orientation = odom_quat;

    //set the velocity
    odom_g.child_frame_id = "base_link";
    odom_g.twist.twist.linear.x = vx;
    odom_g.twist.twist.linear.y = vy;
    odom_g.twist.twist.angular.z = vth;

    odom_publisher_g.publish(odom_g);

    if (status.timestamp - last_cmd_ts > VEL_TIMEOUT_SEC * 1000000.0)
    {
        cmd.x_velocity = 0.;
        cmd.y_velocity = 0.;
        cmd.yaw_velocity = 0.;
    }

    /* if (cntr % 100 == 0)
       {
    if (1)
    {
      printf("********************************\n");
      printf("timestamp: %ld\n", (status.timestamp - last_cmd_ts)/1000000);
      //printf("to: %ld\n", VEL_TIMEOUT_NS);
      {
        //printf("JOINT %d\n", i);
        printf("------------------------------\n");
        printf("X: %f\n",status.x);
        printf("Y: %f\n", status.y);
        printf("YAW: %f\n", status.yaw);
        printf("Vx: %f\n", odom_g.twist.twist.linear.x);
        printf("Vy: %f\n", odom_g.twist.twist.linear.y);
        printf("Va: %f\n", odom_g.twist.twist.angular.z);
         printf("------------------------------\n");
        printf("\n");
      }
    }
       }

       if (cntr % 100 == 0)
       {
    if (1)
    {
      printf("********************************\n");
      printf("timestamp: %ld\n", status.timestamp);
      {
        //printf("JOINT %d\n", i);
        printf("------------------------------\n");
        printf("X: %f\n", odom_g.pose.pose.position.x);
        printf("Y: %f\n", odom_g.pose.pose.position.y);
        printf("YAW: %f\n", th);
        printf("Vx: %f\n", odom_g.twist.twist.linear.x);
        printf("Vy: %f\n", odom_g.twist.twist.linear.y);
        printf("Va: %f\n", odom_g.twist.twist.angular.z);
         printf("------------------------------\n");
        printf("\n");
      }
    }
       }*/


}