底盘与云台通过坐标轴来模拟，目标方块与子弹可视化通过marker仿真。
其中底盘与云台固连，底盘xy方向移动云台会同步移动，云台可进行pitch和yaw轴旋转，通过简单算法可实现云台跟随目标点，再发射出模拟子弹通过物理推导坠落进行模拟击打。
图片：
#!/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标签，再运行仿真代码即可。