rviz仿真底盘移动与云台击打

rviz仿真底盘移动与云台击打

底盘与云台通过坐标轴来模拟，目标方块与子弹可视化通过marker仿真。
其中底盘与云台固连，底盘xy方向移动云台会同步移动，云台可进行pitch和yaw轴旋转，通过简单算法可实现云台跟随目标点，再发射出模拟子弹通过物理推导坠落进行模拟击打。

图片：

rviz仿真代码

#!/usr/bin/env python 
import roslib
import rospy
import tf
import math
from geometry_msgs.msg import Twist
from visualization_msgs.msg import *
from geometry_msgs.msg import Point
current_time,last_time,delta_time=0,0,0
x,y=0,0				#initializes chassis and gimabal position
x0,y0=0,0			#initializes the trajectory of the bullet
twist_x,angular_z=0,0
thetachassis=0
position_x,position_y,position_z=1,2,3	#initializes the position of the strike cube
id,i = 0,0

#teleop control callback
def callback(twist):
    global twist_x
    global angular_z
    twist_x = twist.linear.x
    angular_z = twist.angular.z
    #rospy.loginfo("%f",twist.linear.x)
    #rospy.loginfo("%f",twist_x)
    #rospy.loginfo("%f",angular_z) 

# target cube callback
def positioncallback(position):
	global position_x
	global position_y
	global position_z
	rospy.loginfo("%f  %f  %f",position.x,position.y,position.z)
	position_x = position.x
	position_y = position.y
	position_z = position.z

if __name__=="__main__":
	rospy.init_node('tf_broadcaster')
	rospy.Subscriber('cmd_vel', Twist, callback)
	topic = 'visualization_marker_array'
	topic3 = 'draw_bullet_tracks'
	topic4 = 'draw_target_points'
	publisher = rospy.Publisher(topic, MarkerArray,queue_size = 100)
	publisherpoint = rospy.Publisher(topic4, Marker,queue_size = 100)
	count = 0
	markerArray = MarkerArray()
	rospy.Subscriber("point3d", Point, positioncallback)
	while not rospy.is_shutdown():
		#target strike cube visualization
		marker1 = Marker()
		marker1.header.frame_id = "/world"
		marker1.type = Marker.CUBE
		marker1.action = Marker.ADD
		marker1.ns = 'Testline'
		marker1.scale.x = 0.5
		marker1.scale.y = 0.5
		marker1.scale.z = 0.5
		marker1.color.a = 1.0
		marker1.color.r = 1.0
		marker1.color.g = 1.0
		marker1.color.b = 0.0
		marker1.pose.orientation.w = 1.0
		marker1.pose.position.x = position_x
		marker1.pose.position.y = position_y
		marker1.pose.position.z = position_z
		marker1.id=0
		publisherpoint.publish(marker1)
		
		#get delta time
		last_time=current_time
		current_time = rospy.Time.now().to_sec()
		delta_time=current_time-last_time

		#create gimbal and chassis axes
		br1 = tf.TransformBroadcaster()
		br2 = tf.TransformBroadcaster()
		thetachassis=thetachassis+angular_z*delta_time
		x=x+delta_time*twist_x*math.cos(thetachassis)
		y=y+delta_time*twist_x*math.sin(thetachassis)
		
		#get gimabal pitch and yaw
		ninea_y=position_y-y
		ninea_x=position_x-x
		ninea_z=position_z-1
		pitch=math.atan2((ninea_y),(ninea_x))
		s=math.sqrt(position_x*position_x+position_y*position_y)
		v=10
  		yaw=-math.atan((2*math.pow(v,2)*s-math.sqrt(4*math.pow(v,4)*math.pow(s,2)-8*9.8*math.pow(v,2)*math.pow(s,2)*position_z-4*9.8*9.8*math.pow(s,4)))/(2*9.8*math.pow(s,2)))+0.1
		
		#send the tf transform
		br1.sendTransform((x,y,1.0),
						tf.transformations.quaternion_from_euler(0, yaw,pitch),
						rospy.Time.now(),
						"gimbal",
						"world")
		br2.sendTransform((x,y,0.0),
						tf.transformations.quaternion_from_euler(0, 0,thetachassis),
						rospy.Time.now(),
						"chassis",
						"world")
		if i ==0:
			yaw0,pitch0=yaw,pitch
			i+=1
		#bullet visualization
		marker = Marker()
  		marker.header.frame_id = "/world"
		marker.type = marker.SPHERE
		marker.action = marker.ADD
		marker.scale.x = 0.1
		marker.scale.y = 0.1
		marker.scale.z = 0.1
		marker.color.a = 1.0
		marker.color.r = 1.0
		marker.color.g = 1.0
		marker.color.b = 0.0
		marker.pose.orientation.w = 1.0
		#simulate time
		if(count>1):
			count=0
			print("refresh bullet")
			#get current position and posture
			x0,y0=x,y
			yaw0,pitch0=yaw,pitch
		marker.pose.position.x = v*math.cos(yaw0)*(1-math.pow(2.71828,-count))*math.cos(pitch0)+x0
		marker.pose.position.y = v*math.cos(yaw0)*(1-math.pow(2.71828,-count))*math.sin(pitch0)+y0
		marker.pose.position.z = (9.8+v*math.sin(-yaw))*(1-math.pow(2.71828,-count))-9.8*count+1
		markerArray.markers.append(marker)
		#amend id
		for m in markerArray.markers:
			m.id = id
			id += 1
			if id == 10:
				id=5
		publisher.publish(markerArray)
		count += 0.01
		rospy.sleep(0.01)

键盘控制代码

    #include <termios.h>
    #include <signal.h>
    #include <math.h>
    #include <stdio.h>
    #include <stdlib.h>
    #include <sys/poll.h>
     
    #include <boost/thread/thread.hpp>
    #include <ros/ros.h>
    #include <geometry_msgs/Twist.h>
     
    #define KEYCODE_W 0x77
    #define KEYCODE_A 0x61
    #define KEYCODE_S 0x73
    #define KEYCODE_D 0x64
     
    #define KEYCODE_A_CAP 0x41
    #define KEYCODE_D_CAP 0x44
    #define KEYCODE_S_CAP 0x53
    #define KEYCODE_W_CAP 0x57
     
    class SmartCarKeyboardTeleopNode
    {
        private:
            double walk_vel_;
            double run_vel_;
            double yaw_rate_;
            double yaw_rate_run_;
            
            geometry_msgs::Twist cmdvel_;
            ros::NodeHandle n_;
            ros::Publisher pub_;
     
        public:
            SmartCarKeyboardTeleopNode()
            {
                pub_ = n_.advertise<geometry_msgs::Twist>("cmd_vel", 1);
                
                ros::NodeHandle n_private("~");
                n_private.param("walk_vel", walk_vel_, 0.5);
                n_private.param("run_vel", run_vel_, 1.0);
                n_private.param("yaw_rate", yaw_rate_, 1.0);
                n_private.param("yaw_rate_run", yaw_rate_run_, 1.5);
            }
            
            ~SmartCarKeyboardTeleopNode() { }
            void keyboardLoop();
            
            void stopRobot()
            {
                cmdvel_.linear.x = 0.0;
                cmdvel_.angular.z = 0.0;
                pub_.publish(cmdvel_);
            }
    };
     
    SmartCarKeyboardTeleopNode* tbk;
    int kfd = 0;
    struct termios cooked, raw;
    bool done;
     
    int main(int argc, char** argv)
    {
        ros::init(argc,argv,"tbk", ros::init_options::AnonymousName | ros::init_options::NoSigintHandler);
        SmartCarKeyboardTeleopNode tbk;
        
        boost::thread t = boost::thread(boost::bind(&SmartCarKeyboardTeleopNode::keyboardLoop, &tbk));
        
        ros::spin();
        
        t.interrupt();
        t.join();
        tbk.stopRobot();
        tcsetattr(kfd, TCSANOW, &cooked);
        
        return(0);
    }
     
    void SmartCarKeyboardTeleopNode::keyboardLoop()
    {
        char c;
        double max_tv = walk_vel_;
        double max_rv = yaw_rate_;
        bool dirty = false;
        int speed = 0;
        int turn = 0;
        
        // get the console in raw mode
        tcgetattr(kfd, &cooked);
        memcpy(&raw, &cooked, sizeof(struct termios));
        raw.c_lflag &=~ (ICANON | ECHO);
        raw.c_cc[VEOL] = 1;
        raw.c_cc[VEOF] = 2;
        tcsetattr(kfd, TCSANOW, &raw);
        
        puts("Reading from keyboard");
        puts("Use WASD keys to control the robot");
        puts("Press Shift to move faster");
        
        struct pollfd ufd;
        ufd.fd = kfd;
        ufd.events = POLLIN;
        
        for(;;)
        {
            boost::this_thread::interruption_point();
            
            // get the next event from the keyboard
            int num;
            
            if ((num = poll(&ufd, 1, 250)) < 0)
            {
                perror("poll():");
                return;
            }
            else if(num > 0)
            {
                if(read(kfd, &c, 1) < 0)
                {
                    perror("read():");
                    return;
                }
            }
            else
            {
                if (dirty == true)
                {
                    stopRobot();
                    dirty = false;
                }
                
                continue;
            }
            
            switch(c)
            {
                case KEYCODE_W:
                    max_tv = walk_vel_;
                    speed = 1;
                    turn = 0;
                    dirty = true;
                    break;
                case KEYCODE_S:
                    max_tv = walk_vel_;
                    speed = -1;
                    turn = 0;
                    dirty = true;
                    break;
                case KEYCODE_A:
                    max_rv = yaw_rate_;
                    speed = 0;
                    turn = 1;
                    dirty = true;
                    break;
                case KEYCODE_D:
                    max_rv = yaw_rate_;
                    speed = 0;
                    turn = -1;
                    dirty = true;
                    break;
                    
                case KEYCODE_W_CAP:
                    max_tv = run_vel_;
                    speed = 1;
                    turn = 0;
                    dirty = true;
                    break;
                case KEYCODE_S_CAP:
                    max_tv = run_vel_;
                    speed = -1;
                    turn = 0;
                    dirty = true;
                    break;
                case KEYCODE_A_CAP:
                    max_rv = yaw_rate_run_;
                    speed = 0;
                    turn = 1;
                    dirty = true;
                    break;
                case KEYCODE_D_CAP:
                    max_rv = yaw_rate_run_;
                    speed = 0;
                    turn = -1;
                    dirty = true;
                    break;              
                default:
                    max_tv = walk_vel_;
                    max_rv = yaw_rate_;
                    speed = 0;
                    turn = 0;
                    dirty = false;
            }
            cmdvel_.linear.x = speed * max_tv;
            cmdvel_.angular.z = turn * max_rv;
            pub_.publish(cmdvel_);
        }
    }

放在功能包编译完即可运行，运行时先打开rviz，调出marker和tf标签，再运行仿真代码即可。