I want to manipulate my robot arm (UR5e) to perform pick & place task
so I use ROS and Moveit on Ubuntu 18.04 and I refer to Moveit tutorials (link below)
https://github.com/ros-planning/moveit_tutorials/blob/melodic-devel/doc/move_group_interface/src/move_group_interface_tutorial.cpp
And this is part of my code
const std::vector<double> CAMERA_JOINT = {-1.57899937, -1.63659524, -1.02328654, -2.0525072, 1.57446152, 0.0};
const std::vector<double> HOME_JOINT = {-3.14159265, -1.01839962, -1.43169359, -2.26212124, 1.57376339, 0.0};
class MyRobotPlanning
{
private:
const std::string PLANNING_GROUP = "manipulator";
const robot_state::JointModelGroup* joint_model_group;
moveit::planning_interface::MoveGroupInterface move_group;
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
public:
MyRobotPlanning(): move_group(PLANNING_GROUP)
{
move_group.allowReplanning(true);
move_group.setNumPlanningAttempts(10);
}
void goToGoalPose(geometry_msgs::Pose &pose);
void goToJointState(std::vector<double> setting_values);
};
bool robotMove(capston::FeedBack::Request &req,
capston::FeedBack::Response &res)
{
MyRobotPlanning UR;
cout << "ready to sort your tools!!" << endl;
if (req.next_action == 1)
{
UR.goToJointState(CAMERA_JOINT);
res.feed_back = true;
return true;
}
else if(req.next_action == 2)
{
ros::NodeHandle nh;
ros::ServiceClient client = nh.serviceClient<capston::GraspPose>("grasppose");
capston::GraspPose g_pose;
if (client.call(g_pose))
{
geometry_msgs::Pose goal_pose;
goal_pose.position.x = g_pose.response.grasp_pose.position.x;
goal_pose.position.y = g_pose.response.grasp_pose.position.y;
goal_pose.position.z = g_pose.response.grasp_pose.position.z;
goal_pose.orientation.w = 1.0;
UR.goToGoalPose(goal_pose);
ROS_INFO("Tool Number: %d", (int)g_pose.response.tool_number);
ROS_INFO("tool_pose: [%f, %f, %f]", g_pose.response.grasp_pose.position.x,
g_pose.response.grasp_pose.position.y,
g_pose.response.grasp_pose.position.z);
return true;
}
else
{
ROS_ERROR("Failed to call service");
return false;
}
}
cout << "This code is not complete";
return true;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "action_part");
ros::NodeHandle nh;
ros::AsyncSpinner spinner(1);
spinner.start();
ros::ServiceServer server = nh.advertiseService("feedback", robotMove);
ROS_INFO("Ready srv server!!!");
ros::spin();
return 0;
}
void MyRobotPlanning::goToGoalPose(geometry_msgs::Pose &pose)
{
move_group.setPoseTarget(pose);
ros::Duration(0.5).sleep();
move_group.move();
}
void MyRobotPlanning::goToJointState(std::vector<double> setting_values)
{
move_group.setJointValueTarget(setting_values);
ros::Duration(0.5).sleep();
move_group.move();
}
This is both server and client node
so it receives xyz coordinates of the object (that we want to grasp) from another node
and also receive next_action from the other node and then move my UR5e
When it receives the service feedback.request.next_action = 1 so it calls MyRobotPlanning::goToJointState function and my UR5e moves to CAMERA_JOINT position successfully but it doesn't progress any more
I think there's something stuck in move_group.move() but I don't know why
Related
I have just started using C++ for some image processing tasks. I want to integrate my RGB (OpenCV Mat) and Depth (PCL) data which I get from ros::Subscribe into colored-pointcloud data.
I use the cv::Mat acquiredImage to hold the transmitted image from ros::Subscribe and then the Mat acquiredImage is used for another processes in another threads, but I am facing segmentation fault. Or the error is shown like this:
[xcb] Unknown sequence number while processing queue
[xcb] Most likely this is a multi-threaded client and XInitThreads has not been called
[xcb] Aborting, sorry about that. Viewtest: ../../src/xcb_io.c:260:
poll_for_event: Assertion `!xcb_xlib_threads_sequence_lost' failed.
Aborted (core dumped)
I have tried using std::mutex but it still doesn't work. Could anyone tell me how to properly manage the cv::Mat in two different threads?
typedef pcl::PointXYZRGB XYZRGB;
typedef pcl::PointCloud<XYZRGB> pclXYZRGB;
typedef pcl::PointCloud<XYZRGB>::Ptr pclXYZRGBptr;
typedef pcl::PointCloud<XYZRGB>::ConstPtr pclXYZRGBcptr;
pclXYZRGBptr acquiredCloud (new pclXYZRGB());
pclXYZRGBptr acquiredCloud2 (new pclXYZRGB());
cv::Mat acquiredImage, acquiredImageRotated;
std::thread thread_RunThread;
std::mutex mutexMutex;
bool stopThread, has_data1, has_data2;
inline float PackRGB(uint8_t r, uint8_t g, uint8_t b) {
uint32_t color_uint = ((uint32_t)r << 16 | (uint32_t) g << 8 | (uint32_t)b);
return *reinterpret_cast<float *> (&color_uint);
}
void RunThread(){
while(ros::ok()){
ros::spinOnce();
}
}
void imageReceive(const sensor_msgs::ImageConstPtr& msg){
mutexMutex.lock();
acquiredImage = cv::Mat(cv_bridge::toCvShare(msg, "bgr8")->image);
mutexMutex.unlock();
has_data1 = true;
}
void cloudReceive(const sensor_msgs::PointCloud2ConstPtr& cloudInMsg){
//mutexMutex.lock();
pcl::fromROSMsg(*cloudInMsg, *acquiredCloud);
has_data2 = true;
//mutexMutex.unlock();
}
void StartThread(){
stopThread = false;
has_data1 = has_data2 = false;
thread_RunThread = std::thread(RunThread);
while(!has_data1 && !has_data2){
std::this_thread::sleep_for(std::chrono::milliseconds(1));
std::cout << has_data1 << "-" << has_data2 << std::endl;
}
}
void CloseThread(){
stopThread = true;
thread_RunThread.join();
}
int main(int argc, char **argv){
ros::init(argc, argv, "Viewtest");
ros::NodeHandle nh;
image_transport::ImageTransport it(nh);
cv::startWindowThread();
image_transport::Subscriber sub = it.subscribe("/rsCamera/image", 1, imageReceive);
ros::Subscriber pclsubAcquirer = nh.subscribe("/rsCamera/cloud", 1, cloudReceive);
StartThread();
while (ros::ok()){
if(!has_data1 && !has_data2){
std::this_thread::sleep_for(std::chrono::milliseconds(1));
std::cout << has_data1 << "-" << has_data2 << std::endl;
}
else {
mutexMutex.lock();
cv::rotate(acquiredImage, acquiredImageRotated, cv::ROTATE_180);
mutexMutex.unlock();
copyPointCloud(*acquiredCloud, *acquiredCloud2);
int i = 640, j = 480, k;
for (auto& it : acquiredCloud2->points){
it.x = it.x; it.y = it.y; it.z = it.z;
it.rgb = PackRGB(
acquiredImageRotated.at<cv::Vec3b>(j,i)[2], // r
acquiredImageRotated.at<cv::Vec3b>(j,i)[1], // g
acquiredImageRotated.at<cv::Vec3b>(j,i)[0] // b
);
i--;
if(i <= 0) { i = 640; j--; }
if(j < 0) { break; }
}
}
}
CloseThread();
return 0;
}
I've written a engine for the game "draughts" some time ago and now I want to write a program that communicates with two of those engines via some protocol. In the end I hope to have something similar to the UCI-protocol which is widely known among programmers of chess engines.
The engine is supposed to receive all the commands via stdin and sends it's response via stdout.
I've created some dummy engine to test this with some testing before the if-statement to see if the engine receives anything at all.
int main(){
std::cerr<<"MainEngine"<<std::endl;
while (!std::cin.eof()) {
std::string current;
std::getline(std::cin, current);
std::cerr<<"FromMain:"<<current<<std::endl;
if (current == "init") {
initialize();
std::cout << "ready" << "\n";
} else if (current == "hashSize") {
std::string hash;
std::getline(std::cin, hash);
setHashSize(1u << std::stoi(hash));
} else if (current == "position") {
std::string position;
std::getline(std::cin, position);
} else if (current == "move") {
std::string move;
std::getline(std::cin, move);
}
}
return 0
}
and here is my attempt at the communication-part using pipes
struct Interface {
enum State {
Idle, Ready, Searching
};
const int &engineRead1;
const int &engineRead2;
const int &engineWrite1;
const int &engineWrite2;
State oneState;
State twoState;
void initEngines();
void writeMessage(const int &pipe, const std::string &message);
void processInput(const int &readPipe);
Interface &operator<<(const std::string message);
};
void Interface::processInput(const int &readPipe) {
std::string message;
char c;
while ((read(readPipe, &c, sizeof(char))) != -1) {
if (c == '\n') {
break;
} else {
message += c;
}
}
if (message == "ready") {
std::cout << "ReadyEngine" << std::endl;
}
}
void Interface::writeMessage(const int &pipe, const std::string &message) {
write(pipe, (char *) &message.front(), sizeof(char) * message.size());
}
int main(int argl, const char **argc) {
int numEngines = 2;
int mainPipe[numEngines][2];
int enginePipe[numEngines][2];
for (auto i = 0; i < numEngines; ++i) {
pipe(mainPipe[i]);
pipe(enginePipe[i]);
auto pid = fork();
if (pid < 0) {
std::cerr << "Error" << std::endl;
exit(EXIT_FAILURE);
} else if (pid == 0) {
dup2(mainPipe[i][0], STDIN_FILENO);
close(enginePipe[i][1]);
dup2(enginePipe[i][1], STDOUT_FILENO);
close(mainPipe[i][0]);
execlp("./engine", "engine", NULL);
}
close(enginePipe[i][0]);
close(mainPipe[i][1]);
std::string message = "init\n";
Interface inter{enginePipe[0][0], enginePipe[1][0], mainPipe[0][1], mainPipe[1][1]};
inter.writeMessage(inter.engineWrite1, message);
inter.writeMessage(inter.engineWrite2, message);
int status;
for (int k = 0; k < numEngines; ++k) {
wait(&status);
}
}
}
I am creating two child-process one for each engine. In this test I simply send "init\n" to each of the engine and would expect the child processes to print "FromMain: init". However, I am only getting the output "MainEngine" from one of the child-processes.
This is my first attempt at using pipes and I dont know where I messed up. I would appreciate some tips/help on how to properly setup the communication part.
close(enginePipe[i][1]);
dup2(enginePipe[i][1], STDOUT_FILENO);
You're closing a pipe and then trying to dup it. This doesn't work.
close(enginePipe[i][0]);
close(mainPipe[i][1]);
std::string message = "init\n";
Interface inter{enginePipe[0][0], enginePipe[1][0], mainPipe[0][1], mainPipe[1][1]};
And you're closing these pipes then trying to use them too. And making inter with all of the pipes each iteration through, instead of each only once.
I'd advise you to do something simple with two processes and one pipe, before trying complicated things like this with three processes and four pipes.
When i do the initialisation of the TCPServer inside of main it works, when i try to start it with the startServer() function it is not working, i mean i can not establish a connection with putty.
What am i doing wrong here?
Thanks for any help.
class EchoConnection : public TCPServerConnection {
public:
EchoConnection(const StreamSocket& s)
: TCPServerConnection(s) {}
void reply(char buffer[])
{
bzero(buffer, 256);
std::string myWord = "myWord\n\r";
strcpy(buffer, myWord.c_str());
}
void run() {
StreamSocket& ss = socket();
try {
char buffer[256];
int n = ss.receiveBytes(buffer, sizeof(buffer));
while (n > 0) {
reply(buffer);
ss.sendBytes(buffer, sizeof(buffer));
n = ss.receiveBytes(buffer, sizeof(buffer));
}
}
catch (Poco::Exception& exc)
{ std::cerr << "EchoConnection: " << exc.displayText() << std::endl; }
}
};
void startServer()
{
Poco::Net::TCPServer srv(new Poco::Net::TCPServerConnectionFactoryImpl<EchoConnection>, 8089);
srv.start();
SocketAddress sa("localhost", srv.socket().address().port());
StreamSocket ss(sa);
std::string data("hello, world");
ss.sendBytes(data.data(), (int)data.size());
char buffer[256] = { 0 };
int n = ss.receiveBytes(buffer, sizeof(buffer));
std::cout << std::string(buffer, n) << std::endl;
}
int main(int argc, char** argv) {
Poco::Net::TCPServer srv(new Poco::Net::TCPServerConnectionFactoryImpl<EchoConnection>, 8089);
srv.start();
SocketAddress sa("localhost", srv.socket().address().port());
StreamSocket ss(sa);
std::string data("hello, world");
ss.sendBytes(data.data(), (int)data.size());
char buffer[256] = { 0 };
int n = ss.receiveBytes(buffer, sizeof(buffer));
std::cout << std::string(buffer, n) << std::endl;
// startServer(8089);
ModuleData modData;
modData.ModuleNumber = 1;
modData.ModuleTypeId = 1;
string test = modData.serialize();
ifstream dataFile;
dataFile.open("ModuleData.dat");
if (dataFile.is_open())
{
string line;
while (getline(dataFile, line))
{
cout << line << std::endl;
}
dataFile.close();
}
while (1)
{
}
srv.stop();
}
At the end of startServer function srv object is deleted. TCPServer uses own thread but it finishes working in destructor of TCPServer.
Look at implementation of ~TCPServer
TCPServer::~TCPServer() {
try {
stop();
_pDispatcher->release();
...
}
and see stop method implementation
void TCPServer::stop() {
if (!_stopped)
{
_stopped = true; // !!
_thread.join(); // waiting for thread
_pDispatcher->stop();
}
}
the body of thread function is in run method
void TCPServer::run()
{
while (!_stopped) // this flag was set by stop method
{
... // body of thread function
}
}
stop method sets _stopped on true, and for this reason thread finishes working.
I'm using pjsip high-level api PJSUA for doing multiple calls at one time. My sip operator support up to 3 outgoing lines for calls, so in theory it should be possible to call 3 persons at one time. The problem is, that in the operator logs i see, the message "Line limit overreached" and in pjsip, the call fails. I'm doing it like this:
Here is my class that contains methods for calling:
void Sip::InitLibrary()
{
pj::EpConfig ep_cfg;
ep_cfg.logConfig.level = 4;
ep_cfg.logConfig.consoleLevel = 4;
ep_cfg.logConfig.filename = this->log_;
ep_cfg.uaConfig.userAgent = "Ekiga";
this->ep_.libCreate();
this->ep_.libInit(ep_cfg);
}
void Sip::SetSipTransport(Sip::SipTransport transport, unsigned port)
{
TransportConfig tcfg;
tcfg.port = port;
pjsip_transport_type_e type;
switch (transport)
{
case SipTransport::TCP:
type = PJSIP_TRANSPORT_TCP;
break;
case SipTransport::UDP:
type = PJSIP_TRANSPORT_UDP;
break;
}
try {
this->transportId_ = this->ep_.transportCreate(type, tcfg);
}
catch (Error &err) {
//to log
return;
}
}
void Sip::StartLibrary()
{
this->ep_.libStart();
}
AccountInfo Sip::ConnectSipServer(string serv, string login, string pass,SipTransport transport,int port)
{
this->InitLibrary();
this->SetSipTransport(transport, port);
this->StartLibrary();
this->server_ = serv;
AccountConfig accConfig;
string uri = "<sip:" + login + "#" + serv +">";
accConfig.idUri = uri;
string regUri = "sip:" + serv;
accConfig.regConfig.registrarUri = regUri;
accConfig.sipConfig.authCreds.push_back(AuthCredInfo("digest", "*", login, 0, pass));
this->acc_ = new SipAccount();
acc_->create(accConfig, true);
{
unique_lock<mutex> locker(acc_->regLock_);
while (!acc_->regEnd_)
acc_->regDone_.wait(locker);
}
return acc_->info_;
}
bool Sip::CallNumber(string phone)
{
Call *call = new SipCall(*this->acc_);
CallOpParam prm(true);
prm.opt.audioCount = 1;
prm.opt.videoCount = 0;
call->makeCall("sip:" + phone + "#" + this->server_, prm);
SipCall *myCall = (SipCall*)call;
{
unique_lock<mutex> locker(myCall->callLock);
while (!myCall->callEnd)
myCall->callDone.wait(locker);
}
if (myCall->validPhone)
{
delete myCall;
return true;
}
delete myCall;
return false;
}
SipCall.h
SipCall(Account &acc, int call_id = PJSUA_INVALID_ID)
: Call(acc, call_id)
{
Acc = (SipAccount *)&acc;
this->callEnd = false;
this->validPhone = false;
}
~SipCall();
virtual void onCallState(OnCallStateParam &prm);
condition_variable callDone;
mutex callLock;
bool callEnd;
bool validPhone;
SipCall.cpp
void SipCall::onCallState(OnCallStateParam &prm)
{
PJ_UNUSED_ARG(prm);
CallInfo ci = getInfo();
std::cout << "*** Call: " << ci.remoteUri << " [" << ci.stateText
<< "]" << std::endl;
if (ci.state == PJSIP_INV_STATE_DISCONNECTED) {
{
unique_lock<mutex> locker(this->callLock);
this->callEnd = true;
this->callDone.notify_one();
}
}
if (ci.state == PJSIP_INV_STATE_CONFIRMED && ci.lastStatusCode == PJSIP_SC_OK)
{ //here i check that phone is valid, and just hang up the call
CallOpParam p;
this->validPhone = true;
this->hangup(p);
}
}
SipCall::~SipCall()
{
}
Since there a 3 lines avaliable im creating a three tasks to call the corresponding numbers like so:
QStringList testCalls; // initialized with numbers
struct call_result{
QString phone;
bool valid;
};
//....
QList<QFuture<call_result>> futureTestList;
while(counter < testCalls.count()) {
for(int i= 0; i < 3; i++,counter++) {
if(counter >= testCalls.count())
break;
QString number = testCalls.at(counter);
QFuture<call_result> futureResult = QtConcurrent::run(this,&sipQtThread::callNumber,number);
futureTestList.append(futureResult);
}
foreach(QFuture<call_result> future,futureTestList) {
future.waitForFinished();
call_result call = future.result();
emit phonePassed(call.phone,call.valid);
}
futureTestList.clear();
}
The correspondig task sipQtThread::callNumber is following:
sipQtThread::call_result sipQtThread::callNumber(QString phone)
{
call_result call;
pj_thread_desc initdec;
pj_thread_t* thread = 0;
pj_status_t status;
status = pj_thread_register(NULL, initdec, &thread);
if (status != PJ_SUCCESS) {
qDebug()<<"pj_thread_register faild:"<<status;
return call;
}
bool result = sip_->CallNumber(phone.toStdString());
call.phone = phone;
call.valid = result;
return call;
}
As you can see here
if (ci.state == PJSIP_INV_STATE_CONFIRMED && ci.lastStatusCode == PJSIP_SC_OK)
{ //here i check that phone is valid, and just hang up the call
CallOpParam p;
this->validPhone = true;
this->hangup(p);
}
I just check if the phone is valid, and call is starting, and the drop it. Can pjsua2 handle multiple line calls? Or even is pjsip supporting multiple outgoing lines calls? Does the thread registered by pj_thread_register needs to be somehow unregistered maybe?
HI!
I am trying to pass whole structure from one program and read it in another using fifo.
I'm using read and write functions. I had a problem with putting my structure into this functions. When I did it and tried to send and receive I get an error (core dump) or I recived some trash. I dont know exactly, where my problem take place (in receiver or sender). How can I send/receive my structure, or what i have wrong in my code.Here is my code...Receiver
struct data
{
char* message;
int size;
vector <times> prog;
};
int if_fifo(char* name)
{
struct stat info;
int score = stat(name,&info);
if(S_ISFIFO(info.st_mode))
{
return 1;
}
else
{
return 0;
}
}
int fifo_in(char* name)
{
data msg;
int pip;
pip = open(name, O_RDONLY | O_NONBLOCK);
while(1)
{
int hr = read(pip,&msg,sizeof(msg));
if(hr != 0)
{
cout << "Message: " << msg.message << endl;
}
}
cout << "O.K." << endl;
return 0;
}
int main(int argc, char** argv) {
int c, status_in, status_out;
char* input;
char* output;
float del;
if(argc < 5)
{
cout << "Za malo podanych parametrow" << endl;
return 1;
}
else
{
while ((c = getopt(argc, argv, "iod:")) != -1)
{
switch (c)
{
case 'i':
input = argv[2];
status_in = if_fifo(input);
break;
case 'o':
output = argv[3];
status_out = if_fifo(output);
break;
case 'd':
del = atof(argv[4]);
break;
case '?':
printf("UKNOWN");
}
}
}
if(status_in == 1)
{
return fifo_in(input);
}
else
{
cout << "It isnt fifo!!" << endl;
}
return 0;
}
And sender:
struct data
{
char* message;
int size;
vector <times> prog;
}msg;
int if_fifo(char* name)
{
struct stat info;
int score = stat(name,&info);
if(S_ISFIFO(info.st_mode))
{
return 1;
}
else
{
return 0;
}
}
int fifo_out(char* name)
{
msg.message = "To jest to!!";
msg.size = sizeof(msg.message);
int pip;
pip = open(name, O_WRONLY);
if( pip == -1 )
{
perror("Error: open( ): ");
return 1;
}
write(pip,&msg,sizeof(msg));
return 0;
}
int main(int argc, char** argv) {
int c, status_out;
char* output;
if(argc < 3)
{
cout << "Za malo podanych parametrow" << endl;
return 1;
}
else
{
while ((c = getopt(argc, argv, "o:")) != -1)
{
switch (c)
{
case 'o':
output = argv[2];
status_out = if_fifo(output);
break;
case '?':
printf("UKNOWN");
}
}
}
if(status_out == 1)
{
return fifo_out(output);
}
return 0;
}
you cannot just send memory structures from one program to another. You have to do whats called 'serialization' ie convert the struct into a byte stream that represents the structure. There are many, many serialization techniques: ASN1/ Ber, XML, JSON, Google protocol buffs, roll your own.
Just so you know why this is. The field message in your struct is actually a pointer, when you send this pointer to another program it points to the same address but in the receiver prgram not the sender. That address likely doesnt exist and certainly does not contain the string you had in the sender program.