Skip to content

Commit 3da1c2b

Browse files
committed
WIP ProgramStateMessage
1 parent 6f87e4b commit 3da1c2b

File tree

10 files changed

+399
-12
lines changed

10 files changed

+399
-12
lines changed

CMakeLists.txt

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -21,8 +21,10 @@ add_library(urcl SHARED
2121
src/comm/server.cpp
2222
src/primary/primary_client.cpp
2323
src/primary/primary_package.cpp
24+
src/primary/program_state_message.cpp
2425
src/primary/robot_message.cpp
2526
src/primary/robot_state.cpp
27+
src/primary/program_state_message/global_variables_update_message.cpp
2628
src/primary/robot_message/error_code_message.cpp
2729
src/primary/robot_message/runtime_exception_message.cpp
2830
src/primary/robot_message/version_message.cpp

examples/primary_pipeline.cpp

Lines changed: 15 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -27,7 +27,7 @@
2727

2828
#include <ur_client_library/comm/pipeline.h>
2929
#include <ur_client_library/comm/producer.h>
30-
#include <ur_client_library/comm/shell_consumer.h>
30+
#include <ur_client_library/primary/primary_shell_consumer.h>
3131
#include <ur_client_library/primary/primary_parser.h>
3232

3333
#ifdef ROS_BUILD
@@ -38,7 +38,7 @@ using namespace urcl;
3838

3939
// In a real-world example it would be better to get those values from command line parameters / a better configuration
4040
// system such as Boost.Program_options
41-
const std::string ROBOT_IP = "192.168.56.101";
41+
const std::string ROBOT_IP = "172.17.0.2";
4242

4343
int main(int argc, char* argv[])
4444
{
@@ -60,7 +60,7 @@ int main(int argc, char* argv[])
6060

6161
// The shell consumer will print the package contents to the shell
6262
std::unique_ptr<comm::IConsumer<primary_interface::PrimaryPackage>> consumer;
63-
consumer.reset(new comm::ShellConsumer<primary_interface::PrimaryPackage>());
63+
consumer.reset(new primary_interface::PrimaryShellConsumer());
6464

6565
// The notifer will be called at some points during connection setup / loss. This isn't fully
6666
// implemented atm.
@@ -70,6 +70,18 @@ int main(int argc, char* argv[])
7070
comm::Pipeline<primary_interface::PrimaryPackage> pipeline(prod, consumer.get(), "Pipeline", notifier);
7171
pipeline.run();
7272

73+
std::this_thread::sleep_for(std::chrono::seconds(2));
74+
75+
std::string script_code = "zero_ftsensor()";
76+
77+
auto program_with_newline = script_code + '\n';
78+
79+
size_t len = program_with_newline.size();
80+
const uint8_t* data = reinterpret_cast<const uint8_t*>(program_with_newline.c_str());
81+
size_t written;
82+
83+
primary_stream.write(data, len, written);
84+
7385
// Package contents will be printed while not being interrupted
7486
// Note: Packages for which the parsing isn't implemented, will only get their raw bytes printed.
7587
while (true)

include/ur_client_library/primary/abstract_primary_consumer.h

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
#include "ur_client_library/primary/robot_message/text_message.h"
3737
#include "ur_client_library/primary/robot_message/version_message.h"
3838
#include "ur_client_library/primary/robot_state/kinematics_info.h"
39+
#include "ur_client_library/primary/program_state_message/global_variables_update_message.h"
3940

4041
namespace urcl
4142
{
@@ -80,6 +81,8 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
8081
virtual bool consume(TextMessage& pkg) = 0;
8182
virtual bool consume(VersionMessage& pkg) = 0;
8283
virtual bool consume(KinematicsInfo& pkg) = 0;
84+
virtual bool consume(ProgramStateMessage& pkg) = 0;
85+
virtual bool consume(GlobalVariablesUpdateMessage& pkg) = 0;
8386

8487
private:
8588
/* data */

include/ur_client_library/primary/primary_parser.h

Lines changed: 39 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -32,6 +32,8 @@
3232
#include "ur_client_library/primary/robot_message/runtime_exception_message.h"
3333
#include "ur_client_library/primary/robot_message/text_message.h"
3434
#include "ur_client_library/primary/robot_message/version_message.h"
35+
#include "ur_client_library/primary/program_state_message/global_variables_update_message.h"
36+
3537

3638
namespace urcl
3739
{
@@ -138,6 +140,28 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
138140
break;
139141
}
140142

143+
case RobotPackageType::PROGRAM_STATE_MESSAGE:
144+
{
145+
uint64_t timestamp;
146+
ProgramStateMessageType state_type;
147+
LOG_DEBUG("ProgramStateMessage received");
148+
149+
bp.parse(timestamp);
150+
bp.parse(state_type);
151+
152+
LOG_DEBUG("ProgramStateMessage of type %d received", static_cast<int>(state_type));
153+
154+
std::unique_ptr<PrimaryPackage> packet(programStateFromType(state_type, timestamp));
155+
if (!packet->parseWith(bp))
156+
{
157+
LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(state_type));
158+
return false;
159+
}
160+
161+
results.push_back(std::move(packet));
162+
return true;
163+
}
164+
141165
default:
142166
{
143167
LOG_DEBUG("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
@@ -190,6 +214,21 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
190214
return new RobotMessage(timestamp, source, type);
191215
}
192216
}
217+
218+
ProgramStateMessage* programStateFromType(ProgramStateMessageType type, uint64_t timestamp)
219+
{
220+
switch(type)
221+
{
222+
//case ProgramStateMessageType::GLOBAL_VARIABLES_SETUP:
223+
//return new GlobalVariablesSetupMessage(timestamp);
224+
case ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE:
225+
return new GlobalVariablesUpdateMessage(timestamp);
226+
//case ProgramStateMessageType::TYPE_VARIABLES_UPDATE:
227+
//return new TypeVariablesUpdateMessage(timestamp);
228+
default:
229+
return new ProgramStateMessage(timestamp, type);
230+
}
231+
}
193232
};
194233

195234
} // namespace primary_interface

include/ur_client_library/primary/primary_shell_consumer.h

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -82,6 +82,16 @@ class PrimaryShellConsumer : public AbstractPrimaryConsumer
8282
LOG_INFO("%s", msg.toString().c_str());
8383
return true;
8484
}
85+
virtual bool consume(ProgramStateMessage& msg) override
86+
{
87+
LOG_INFO("---ProgramStateMessage---%s", msg.toString().c_str());
88+
return true;
89+
}
90+
virtual bool consume(GlobalVariablesUpdateMessage& msg) override
91+
{
92+
LOG_INFO("---GlobalVariablesUpdateMessage---\n%s", msg.toString().c_str());
93+
return true;
94+
}
8595

8696
private:
8797
/* data */
Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2+
3+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
4+
// Copyright 2019 FZI Forschungszentrum Informatik
5+
//
6+
// Licensed under the Apache License, Version 2.0 (the "License");
7+
// you may not use this file except in compliance with the License.
8+
// You may obtain a copy of the License at
9+
//
10+
// http://www.apache.org/licenses/LICENSE-2.0
11+
//
12+
// Unless required by applicable law or agreed to in writing, software
13+
// distributed under the License is distributed on an "AS IS" BASIS,
14+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
// See the License for the specific language governing permissions and
16+
// limitations under the License.
17+
// -- END LICENSE BLOCK ------------------------------------------------
18+
19+
//----------------------------------------------------------------------
20+
/*!\file
21+
*
22+
* \author Felix Exner <[email protected]>
23+
* \date 2022-02-21
24+
*
25+
*/
26+
//----------------------------------------------------------------------
27+
28+
#ifndef UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED
29+
#define UR_CLIENT_LIBRARY_PROGRAM_STATE_MESSAGE_H_INCLUDED
30+
31+
#include "ur_client_library/primary/primary_package.h"
32+
#include "ur_client_library/primary/package_header.h"
33+
34+
namespace urcl
35+
{
36+
namespace primary_interface
37+
{
38+
/*!
39+
* \brief Possible ProgramStateMessage types
40+
*/
41+
enum class ProgramStateMessageType : uint8_t
42+
{
43+
GLOBAL_VARIABLES_SETUP = 0,
44+
GLOBAL_VARIABLES_UPDATE = 1,
45+
TYPE_VARIABLES_UPDATE = 2,
46+
};
47+
48+
/*!
49+
* \brief The ProgramStateMessage class is a parent class for the different received robot messages.
50+
*/
51+
class ProgramStateMessage : public PrimaryPackage
52+
{
53+
public:
54+
/*!
55+
* \brief Creates a new ProgramStateMessage object to be filled from a package.
56+
*
57+
* \param timestamp Timestamp of the package
58+
*/
59+
ProgramStateMessage(const uint64_t timestamp) : timestamp_(timestamp)
60+
{
61+
}
62+
63+
/*!
64+
* \brief Creates a new ProgramStateMessage object to be filled from a package.
65+
*
66+
* \param timestamp Timestamp of the package
67+
* \param message_type The package's message type
68+
*/
69+
ProgramStateMessage(const uint64_t timestamp, const ProgramStateMessageType state_type)
70+
: timestamp_(timestamp), state_type_(state_type)
71+
{
72+
}
73+
virtual ~ProgramStateMessage() = default;
74+
75+
/*!
76+
* \brief Sets the attributes of the package by parsing a serialized representation of the
77+
* package.
78+
*
79+
* \param bp A parser containing a serialized version of the package
80+
*
81+
* \returns True, if the package was parsed successfully, false otherwise
82+
*/
83+
virtual bool parseWith(comm::BinParser& bp);
84+
85+
/*!
86+
* \brief Consume this package with a specific consumer.
87+
*
88+
* \param consumer Placeholder for the consumer calling this
89+
*
90+
* \returns true on success
91+
*/
92+
virtual bool consumeWith(AbstractPrimaryConsumer& consumer);
93+
94+
/*!
95+
* \brief Produces a human readable representation of the package object.
96+
*
97+
* \returns A string representing the object
98+
*/
99+
virtual std::string toString() const;
100+
101+
uint64_t timestamp_;
102+
ProgramStateMessageType state_type_;
103+
};
104+
105+
} // namespace primary_interface
106+
} // namespace urcl
107+
108+
#endif /* UR_CLIENT_LIBRARY_ROBOT_STATE_H_INCLUDED */
Lines changed: 90 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,90 @@
1+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
2+
3+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
4+
// Copyright 2019 FZI Forschungszentrum Informatik
5+
//
6+
// Licensed under the Apache License, Text 2.0 (the "License");
7+
// you may not use this file except in compliance with the License.
8+
// You may obtain a copy of the License at
9+
//
10+
// http://www.apache.org/licenses/LICENSE-2.0
11+
//
12+
// Unless required by applicable law or agreed to in writing, software
13+
// distributed under the License is distributed on an "AS IS" BASIS,
14+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
15+
// See the License for the specific language governing permissions and
16+
// limitations under the License.
17+
// -- END LICENSE BLOCK ------------------------------------------------
18+
19+
//----------------------------------------------------------------------
20+
/*!\file
21+
*
22+
* \author Felix Exner [email protected]
23+
* \date 20222-02-21
24+
*
25+
*/
26+
//----------------------------------------------------------------------
27+
28+
#ifndef UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_H_INCLUDED
29+
#define UR_CLIENT_LIBRARY_PRIMARY_GLOBAL_VARIABLES_UPDATE_MESSAGE_H_INCLUDED
30+
31+
#include "ur_client_library/primary/program_state_message.h"
32+
33+
namespace urcl
34+
{
35+
namespace primary_interface
36+
{
37+
/*!
38+
* \brief The GlobalVariablesUpdateMessage class handles the key messages sent via the primary UR interface.
39+
*/
40+
class GlobalVariablesUpdateMessage : public ProgramStateMessage
41+
{
42+
public:
43+
GlobalVariablesUpdateMessage() = delete;
44+
/*!
45+
* \brief Creates a new GlobalVariablesUpdateMessage object to be filled from a package.
46+
*
47+
* \param timestamp Timestamp of the package
48+
* \param source The package's source
49+
*/
50+
GlobalVariablesUpdateMessage(const uint64_t timestamp)
51+
: ProgramStateMessage(timestamp, ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE)
52+
{
53+
}
54+
virtual ~GlobalVariablesUpdateMessage() = default;
55+
56+
/*!
57+
* \brief Sets the attributes of the package by parsing a serialized representation of the
58+
* package.
59+
*
60+
* \param bp A parser containing a serialized text of the package
61+
*
62+
* \returns True, if the package was parsed successfully, false otherwise
63+
*/
64+
virtual bool parseWith(comm::BinParser& bp);
65+
66+
/*!
67+
* \brief Consume this package with a specific consumer.
68+
*
69+
* \param consumer Placeholder for the consumer calling this
70+
*
71+
* \returns true on success
72+
*/
73+
virtual bool consumeWith(AbstractPrimaryConsumer& consumer);
74+
75+
/*!
76+
* \brief Produces a human readable representation of the package object.
77+
*
78+
* \returns A string representing the object
79+
*/
80+
virtual std::string toString() const;
81+
82+
uint16_t start_index_;
83+
std::string variables_;
84+
85+
};
86+
} // namespace primary_interface
87+
} // namespace urcl
88+
89+
#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED
90+

src/primary/primary_client.cpp

Lines changed: 11 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -41,19 +41,21 @@ PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& cal
4141
producer_.reset(new comm::URProducer<PrimaryPackage>(*stream_, parser_));
4242
producer_->setupProducer();
4343

44-
consumer_.reset(new PrimaryConsumer());
45-
std::shared_ptr<CalibrationChecker> calibration_checker(new CalibrationChecker(calibration_checksum));
46-
consumer_->setKinematicsInfoHandler(calibration_checker);
44+
//consumer_.reset(new PrimaryConsumer());
45+
//std::shared_ptr<CalibrationChecker> calibration_checker(new CalibrationChecker(calibration_checksum));
46+
//consumer_->setKinematicsInfoHandler(calibration_checker);
4747

48+
std::unique_ptr<comm::IConsumer<primary_interface::PrimaryPackage>> consumer;
49+
consumer.reset(new primary_interface::PrimaryShellConsumer());
4850
pipeline_.reset(new comm::Pipeline<PrimaryPackage>(*producer_, consumer_.get(), "primary pipeline", notifier_));
4951
pipeline_->run();
5052

51-
calibration_checker->isChecked();
52-
while (!calibration_checker->isChecked())
53-
{
54-
std::this_thread::sleep_for(std::chrono::seconds(1));
55-
}
56-
LOG_DEBUG("Got calibration information from robot.");
53+
//calibration_checker->isChecked();
54+
//while (!calibration_checker->isChecked())
55+
//{
56+
//std::this_thread::sleep_for(std::chrono::seconds(1));
57+
//}
58+
//LOG_DEBUG("Got calibration information from robot.");
5759
}
5860

5961
bool PrimaryClient::sendScript(const std::string& script_code)

0 commit comments

Comments
 (0)