我有一个JAVA程序，用于保存由华硕Xtion Pro Live相机生成的图像，我想将存储图像的路径名发送到KUKA youBOT。在youBOT完成移动手臂之后，我想将该路径名称发送给Torch。路径名称的格式为“/home/.../classes/100.jpg”。
JAVA代码是：
if(xxx!=0){
Highgui.imwrite(x1+"-.jpg", out);
String name = "/home/isa/Downloads/java_rosbridge/target/classes/" + x1 + ".jpg";
System.out.println("name:" + x1);
String name1 = String.valueOf(x1);
pubname.publish(new PrimitiveMsg<String>(name1));
String ttttx=String.valueOf(XXXX);
String tttty=String.valueOf(YYYY);
String ttttz=String.valueOf(ZZZZ);
pubx.publish(new PrimitiveMsg<String>(ttttx));
puby.publish(new PrimitiveMsg<String>(tttty));
pubz.publish(new PrimitiveMsg<String>(ttttz));
delay(500);
//Highgui.imwrite(x1+"-.jpg", out2);
//System.exit(0);
System.out.println(name);
}
youBOT的c ++代码是：
double x;
double y;
double z;
double name;
void chatterCallbackName(const std_msgs::String::ConstPtr& msg) {
const char *namestring = msg->data.c_str();
name = atof(namestring);
ROS_INFO("Name Callback: %s",name);
}
int main(int argc, char **argv) {
bool booleanname;
ros::init(argc, argv, "moveit_test");
ros::NodeHandle n;
ros::AsyncSpinner spinner(1);
spinner.start();
// The :move_group_interface:`MoveGroup` class can be easily setup using just the name of the group you would like to control and plan for.
moveit::planning_interface::MoveGroup group("arm_1");
// print the name of the reference frame for this robot.
ROS_INFO("Reference frame: %s", group.getPlanningFrame().c_str());
// print the name of the end-effector link for this group.
std::string endEffector = group.getEndEffectorLink();
ROS_INFO("Reference frame: %s", endEffector.c_str());
const robot_state::JointModelGroup* joint_model_group = group.getCurrentState()->getRobotModel()->getJointModelGroup(group.getName());
float phi [] = {-180, -180, 0}; //154
float theta [] = {-90, -67, 160}; //90
float psi [] = {0, -6, -5}; //39
ros::Subscriber subname = n.subscribe("xtionname", 1000, chatterCallbackName);
/*
ros::Subscriber subxl = n.subscribe("luax", 1000, chatterCallbackx);
ros::Subscriber subyl = n.subscribe("luay", 1000, chatterCallbacky);
ros::Subscriber subzl = n.subscribe("luaz", 1000, chatterCallbackz);
*/
for (int iter=0; iter<2; ++iter)
{
bool boolean;
phi[iter] = phi[iter] * (M_PI/180); theta[iter] = theta[iter] * (M_PI/180); psi[iter] = psi[iter] * (M_PI/180);
float qx = cos(phi[iter]/2)*cos(theta[iter]/2)*cos(psi[iter]/2) + sin(phi[iter]/2)*sin(theta[iter]/2)*sin(psi[iter]/2);
float qy = sin(phi[iter]/2)*cos(theta[iter]/2)*cos(psi[iter]/2) - cos(phi[iter]/2)*sin(theta[iter]/2)*sin(psi[iter]/2);
float qz = cos(phi[iter]/2)*sin(theta[iter]/2)*cos(psi[iter]/2) + sin(phi[iter]/2)*cos(theta[iter]/2)*sin(psi[iter]/2);
float qw = cos(phi[iter]/2)*cos(theta[iter]/2)*sin(psi[iter]/2) - sin(phi[iter]/2)*sin(theta[iter]/2)*cos(psi[iter]/2);
const double des_posX[] = {0.35, 0.35 + x, 0.18652}; // , , 0.543175
const double des_posY[] = {-0.05, -y, -0.00384585}; // , , -0.0720582
const double des_posZ[] = {0.25, 0.25 - z, 0.371596}; // , , 0.161611
const double des_orientX[] = {qx, qx, qx}; // -0.03369, 0.0670559, 0.0384911, -0.00385598
const double des_orientY[] = {qy, qy, qy}; // 0.834522, 0.712052, 0.405772, 0.17666
const double des_orientZ[] = {qz, qz, qz}; // -0.0442666, -0.0783854, -0.0863349, -0.211289
const double des_orientW[] = {qw, qw, qw}; // 0.548158, 0.694508, 0.909073, 0.961319
// current pose
geometry_msgs::PoseStamped target_pose1 = group.getCurrentPose();
ROS_INFO("Current position: x: %f, y: %f, z: %f", target_pose1.pose.position.x, target_pose1.pose.position.y, target_pose1.pose.position.z);
ROS_INFO("Current orientation: x: %f, y: %f, z: %f, w: %f", target_pose1.pose.orientation.x, target_pose1.pose.orientation.y, target_pose1.pose.orientation.z, target_pose1.pose.orientation.w);
// goal pose
target_pose1.pose.position.x = des_posX[iter];
target_pose1.pose.position.y = des_posY[iter];
target_pose1.pose.position.z = des_posZ[iter];
target_pose1.pose.orientation.x = des_orientX[iter];
target_pose1.pose.orientation.y = des_orientY[iter];
target_pose1.pose.orientation.z = des_orientZ[iter];
target_pose1.pose.orientation.w = des_orientW[iter];
ROS_INFO("Target position: x: %f, y: %f, z: %f", target_pose1.pose.position.x, target_pose1.pose.position.y, target_pose1.pose.position.z);
ROS_INFO("Target orientation: x: %f, y: %f, z: %f, w: %f", target_pose1.pose.orientation.x, target_pose1.pose.orientation.y, target_pose1.pose.orientation.z, target_pose1.pose.orientation.w);
// directly call IK here
bool ik_success = group.setJointValueTarget(target_pose1, endEffector);
ROS_INFO("IK %s",ik_success?"SUCCESS":"FAILURE");
if (ik_success)
{
std::vector<double> joint_values;
group.getCurrentState()->copyJointGroupPositions(joint_model_group, joint_values);
ROS_INFO("Current joint: %f %f %f %f %f", joint_values[0], joint_values[1], joint_values[2], joint_values[3], joint_values[4]);
group.getJointValueTarget().copyJointGroupPositions(joint_model_group, joint_values);
ROS_INFO("Target joint: %f %f %f %f %f", joint_values[0], joint_values[1], joint_values[2], joint_values[3], joint_values[4]);
// now we could also just directly send the desired joint positions to the robot
// just planning, not moving the robot yet
moveit::planning_interface::MoveGroup::Plan my_plan;
bool plan_success = group.plan(my_plan);
ROS_INFO("Planning %s",plan_success?"SUCCESS":"FAILURE");
if (plan_success)
{
// move robot
group.move();
// sleep(5.0);
target_pose1 = group.getCurrentPose();
ROS_INFO("Current position: x: %f, y: %f, z: %f", target_pose1.pose.position.x, target_pose1.pose.position.y, target_pose1.pose.position.z);
ROS_INFO("Current orientation: x: %f, y: %f, z: %f, w: %f", target_pose1.pose.orientation.x, target_pose1.pose.orientation.y, target_pose1.pose.orientation.z, target_pose1.pose.orientation.w);
group.getCurrentState()->copyJointGroupPositions(joint_model_group, joint_values);
ROS_INFO("Current joint: %f %f %f %f %f", joint_values[0], joint_values[1], joint_values[2], joint_values[3], joint_values[4]);
publish();
ROS_INFO("Name: %s",name);
ros::Publisher chatter_pub = n.advertise<std_msgs::String>("path", 1000);
ros::Publisher bool_pub = n.advertise<std_msgs::String>("bool", 1000);
boolean = 1;
while (boolean == 1) {
std_msgs::String msg;
std::stringstream ss;
ss << name;
msg.data = ss.str();
chatter_pub.publish(msg);
ROS_INFO("Path: %s",msg.data.c_str());
ROS_INFO("Path2: %s",name);
booleanname = 0;
std_msgs::String msgbool;
std::stringstream ssbool;
ssbool << "Hallo";
msgbool.data = ssbool.str();
bool_pub.publish(msgbool);
boolean = 0;
}
}
}
}
ros::spin();
// END_TUTORIAL
//ros::shutdown();
//return 0;
}
火炬代码是：
-- plaatjes binnenkrijgen en commando's versturen
--while true do
bool = Hallo
while (bool == Hallo) do
subscribery = nodehandle:subscribe("bool", 'std_msgs/String', 100)
sys.sleep(0.1)
while subscribery:hasMessage() do
bool = subscribery:read()
print(bool)
end
--function sleep(n)
-- os.execute("sleep " .. tonumber(n+1))
--end
end
int = 1
while int < 2 do
subscriberx = nodehandle:subscribe("xtionname", 'std_msgs/String', 100)
while subscriberx:hasMessage() do
msg = subscriberx:read()
print(msg)
end
int = int+1
end
sys.sleep(30.0)
string1 = "/home/isa/Downloads/java_rosbridge/target/classes/"
string2 = "500"
string3 = ".jpg"
plaatje = image.load(string1 .. msg .. string3)
string_specx = ros.MsgSpec('std_msgs/String')
string_specy = ros.MsgSpec('std_msgs/String')
string_specz = ros.MsgSpec('std_msgs/String')
publisherx = nodehandle:advertise("luax", string_specx, 100)
publishery = nodehandle:advertise("luay", string_specy, 100)
publisherz = nodehandle:advertise("luaz", string_specz, 100)
x = ros.Message(string_specx)
y = ros.Message(string_specy)
z = ros.Message(string_specz)
function run(n)
for i = 1,100 do
if not ros.ok() then
return
end
if publisherx:getNumSubscribers() == 0 and publishery:getNumSubscribers() == 0 and publisherz:getNumSubscribers() == 0 then
print('waiting for subscriber')
else
x.data = string.format("%.7f", net:forward(plaatje)[1])
y.data = string.format("%.7f", net:forward(plaatje)[2])
z.data = string.format("%.7f", net:forward(plaatje)[3])
publisherx:publish(x)
publishery:publish(y)
publisherz:publish(z)
return
end
sys.sleep(0.1)
ros.spinOnce()
end
end
run(100)
--end
ros.shutdown()
每当我从youBOT程序中读取路径名时，输出都是某种乱码，即?? !! G @@ !!。但是，如果我对它应用rostopic回声，我会得到没有问题的路径名。另外，如果我在youBOT程序中的CallBack函数中打印路径名，我也会得到预期的输出。
我无法以某种方式让它工作。有人可以帮帮我吗？