List Sequence::asList() const { return List(NewReference(PySequence_List(mPtr))); }
void square_controller::run() { ros::Rate loop_rate(20); const double MAX_TWIST_LINEAR = 0.5; const double MAX_TWIST_ANGULAR = 0.5; while (ros::ok()) { error_lin = ErrorLinear(curr,reff); error_ang = ErrorAngle(curr,reff); error_lin_old += error_lin; error_ang_old += error_ang; twist.angular.z = kp2*sin(error_ang); twist.linear.x = kp1*error_lin; // Saturazione sul twist comandato if(twist.linear.x > MAX_TWIST_LINEAR) twist.linear.x = MAX_TWIST_LINEAR; if(twist.angular.z > MAX_TWIST_ANGULAR) twist.angular.z = MAX_TWIST_ANGULAR; comand_pub.publish(twist); if(error_lin < 0.1) { twist.angular.z = 0; if(error_lin < 0.03) twist.linear.x = 0; if(typeController == 0) NewReference(); } std_msgs::String mess; std::stringstream ss; ss << "\nCurrent: x = " << curr.pose.pose.position.x << " y = " << curr.pose.pose.position.y << " theta = " << curr.pose.pose.orientation.z*180/M_PI << "\n"; ss << "Reference: x = " << reff.x << " y = " << reff.y << " theta = " << reff.theta*180/M_PI << "\n"; ss << "Linear error: " << pow(pow(reff.x-curr.pose.pose.position.x,2)+pow(reff.y-curr.pose.pose.position.y,2),0.5) << " m. \n"; ss << "Angular error: " << (reff.theta-curr.pose.pose.orientation.z)*180/M_PI << " deg. \n"; ss << "Twist: \n" << twist.linear.x << "\n" << twist.linear.y << "\n" << twist.linear.z << "\n" << twist.angular.x << "\n" << twist.angular.y << "\n" << twist.angular.z << " \n"; ss << "Type controller: "; if(typeController) ss<< "Point controller" << "\n"; else ss<< "Auto controller" << "\n"; mess.data = ss.str(); ROS_INFO("%s", mess.data.c_str()); ros::spinOnce(); loop_rate.sleep(); } }
Tuple Sequence::asTuple() const { return Tuple(NewReference(PySequence_Tuple(mPtr))); }
SV *NewReference(PERL_CALL HV *refObj) { return NewReference(P_PERL (SV*)refObj); }