WRS2020用にメカナム台車をROS化
Dependencies: mbed PID2 JY901 QuaternionMath ros_lib_melodic i2cmaster WRS2020_mecanum_node
Dependents: WRS2020_mecanum_node
main.cpp
- Committer:
- sgrsn
- Date:
- 2020-11-12
- Revision:
- 4:40167450cdf0
- Parent:
- 3:cf06189a7511
File content as of revision 4:40167450cdf0:
#include "mbed.h" #include <string> #include "JY901.h" #include "PID.h" #include "define.h" #include "Quaternion.h" #include <ros.h> #include <geometry_msgs/Twist.h> #include <tf/transform_broadcaster.h> #include <tf/tf.h> #include <nav_msgs/Odometry.h> #include <geometry_msgs/Pose2D.h> #include <geometry_msgs/PoseStamped.h> #include <std_msgs/Empty.h> #include <std_srvs/Trigger.h> #include <std_srvs/Empty.h> #include "robot_operator.h" #include "device.h" // To do // まれにmbed実行エラーが発生(LED1点滅) // (my_odomで返すvel_xは差分を取るより周波数から計算した方がいいか。多分良い) // ros::NodeHandle mecanum_node; void cmd_vel_callback(const geometry_msgs::Twist& cmd_vel){ moveStop(); robot_vel_controll(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z); } bool is_stop = false; void cmd_pose2d_callback(const geometry_msgs::Pose2D& cmd_pose2d){ //robot_pose_controll(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta); moveStart(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta, &is_stop); } void goal_callback(const geometry_msgs::PoseStamped& goal){ //robot_pose_controll(cmd_pose2d.x, cmd_pose2d.y, cmd_pose2d.theta); Quaternion tmp_q(goal.pose.orientation.w, goal.pose.orientation.x, goal.pose.orientation.y, goal.pose.orientation.z); moveStart(goal.pose.position.x, goal.pose.position.y, tmp_q.getEulerAngles().x, &is_stop); } void move_stop_service(const std_srvs::Trigger::Request &req, std_srvs::Trigger::Response &res) { res.success = true; res.message = "Stop"; moveStop(); } void odom_update() { my_odometry.update(); } //ros::ServiceServer<geometry_msgs::Pose2D,std_msgs::Empty> start_service("wheel/move_start", &move_start_service); ros::ServiceServer<std_srvs::Trigger::Request, std_srvs::Trigger::Response> stop_service("wheel/move_stop", &move_stop_service); ros::Subscriber<geometry_msgs::Twist> cmd_vel_sub("wheel/cmd_vel", &cmd_vel_callback); //ros::Subscriber<geometry_msgs::Pose2D> cmd_pose_sub("wheel/cmd_pose2d", &cmd_pose2d_callback); ros::Subscriber<geometry_msgs::PoseStamped> goal_sub("/move_base_simple/goal", &goal_callback); int main() { BusOut LEDs(LED1, LED2, LED3, LED4); coastAllMotor(); mecanum_node.initNode(); mecanum_node.subscribe(cmd_vel_sub); mecanum_node.subscribe(goal_sub); mecanum_node.advertiseService(stop_service); nav_msgs::Odometry odom; ros::Publisher odom_pub("wheel/odom", &odom); mecanum_node.advertise(odom_pub); tf::TransformBroadcaster odom_broadcaster; odom_broadcaster.init(mecanum_node); ros::Time current_time, last_time; current_time = mecanum_node.now(); last_time = mecanum_node.now(); Ticker ticker_odom; ticker_odom.attach(&odom_update, 0.010); while (1) { mecanum_node.spinOnce(); current_time = mecanum_node.now(); wait_ms(500); if(is_stop) { LEDs = 0x0F; } else { LEDs = LEDs+1; } //since all odometry is 6DOF we'll need a quaternion created from yaw geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(my_odometry.theta); //first, we'll publish the transform over tf geometry_msgs::TransformStamped odom_trans; odom_trans.header.stamp = current_time; odom_trans.header.frame_id = "odom"; odom_trans.child_frame_id = "base_link"; odom_trans.transform.translation.x = my_odometry.x; odom_trans.transform.translation.y = my_odometry.y; odom_trans.transform.translation.z = 0.0; odom_trans.transform.rotation = odom_quat; //send the transform odom_broadcaster.sendTransform(odom_trans); //next, we'll publish the odometry message over ROS nav_msgs::Odometry odom; odom.header.stamp = current_time; odom.header.frame_id = "odom"; //set the position odom.pose.pose.position.x = my_odometry.x; odom.pose.pose.position.y = my_odometry.y; odom.pose.pose.position.z = 0.0; odom.pose.pose.orientation = odom_quat; //set the velocity odom.child_frame_id = "base_link"; odom.twist.twist.linear.x = my_odometry.vel_x; odom.twist.twist.linear.y = my_odometry.vel_y; odom.twist.twist.angular.z = my_odometry.vel_theta; //publish the message odom_pub.publish(&odom); } }