Skip to content

Commit b68ae26

Browse files
jessica-chen-ocadojessica-chen-ocado
committed
Exposed error code messages
1 parent df9f95c commit b68ae26

File tree

11 files changed

+428
-0
lines changed

11 files changed

+428
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@ add_library(urcl SHARED
2828
src/primary/robot_message.cpp
2929
src/primary/robot_state.cpp
3030
src/primary/robot_message/version_message.cpp
31+
src/primary/robot_message/error_code_message.cpp
3132
src/primary/robot_state/kinematics_info.cpp
3233
src/rtde/control_package_pause.cpp
3334
src/rtde/control_package_setup_inputs.cpp
@@ -41,6 +42,8 @@ add_library(urcl SHARED
4142
src/rtde/rtde_client.cpp
4243
src/ur/ur_driver.cpp
4344
src/ur/calibration_checker.cpp
45+
src/ur/error_code_reader.cpp
46+
src/ur/error_code_client.cpp
4447
src/ur/dashboard_client.cpp
4548
src/ur/tool_communication.cpp
4649
src/ur/robot_receive_timeout.cpp

‎include/ur_client_library/primary/primary_parser.h‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
#include "ur_client_library/primary/robot_message.h"
2929
#include "ur_client_library/primary/robot_state/kinematics_info.h"
3030
#include "ur_client_library/primary/robot_message/version_message.h"
31+
#include "ur_client_library/primary/robot_message/error_code_message.h"
3132

3233
namespace urcl
3334
{
@@ -174,6 +175,8 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
174175
return new MBD;*/
175176
case RobotMessagePackageType::ROBOT_MESSAGE_VERSION:
176177
return new VersionMessage(timestamp, source);
178+
case RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE:
179+
return new ErrorCodeMessage(timestamp, source);
177180
default:
178181
return new RobotMessage(timestamp, source);
179182
}

‎include/ur_client_library/primary/robot_message.h‎

Lines changed: 11 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -65,6 +65,17 @@ class RobotMessage : public PrimaryPackage
6565
*/
6666
RobotMessage(const uint64_t timestamp, const uint8_t source) : timestamp_(timestamp), source_(source)
6767
{
68+
}
69+
/*!
70+
* \brief Creates a new RobotMessage object to be filled from a package.
71+
*
72+
* \param timestamp Timestamp of the package
73+
* \param source The package's source
74+
* \param message_type The package's message type
75+
*/
76+
RobotMessage(const uint64_t timestamp, const int8_t source, const RobotMessagePackageType message_type)
77+
: timestamp_(timestamp), source_(source), message_type_(message_type)
78+
{
6879
}
6980
virtual ~RobotMessage() = default;
7081

‎include/ur_client_library/primary/robot_message/error_code_message.h‎

Lines changed: 122 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,122 @@
1+
// Below code is copied from a branch on the Universal_Robot_Client_Library
2+
// with some modification. Link to branch:
3+
// https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface
4+
//
5+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
6+
7+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
8+
// Copyright 2019 FZI Forschungszentrum Informatik
9+
//
10+
// Licensed under the Apache License, Text 2.0 (the "License");
11+
// you may not use this file except in compliance with the License.
12+
// You may obtain a copy of the License at
13+
//
14+
// http://www.apache.org/licenses/LICENSE-2.0
15+
//
16+
// Unless required by applicable law or agreed to in writing, software
17+
// distributed under the License is distributed on an "AS IS" BASIS,
18+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
19+
// See the License for the specific language governing permissions and
20+
// limitations under the License.
21+
// -- END LICENSE BLOCK ------------------------------------------------
22+
23+
//----------------------------------------------------------------------
24+
/*!\file
25+
*
26+
* \author Felix Exner [email protected]
27+
* \date 2020-04-23
28+
*
29+
*/
30+
//----------------------------------------------------------------------
31+
32+
#ifndef UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED
33+
#define UR_CLIENT_LIBRARY_PRIMARY_ERROR_CODE_MESSAGE_H_INCLUDED
34+
35+
#include "ur_client_library/primary/robot_message.h"
36+
37+
namespace urcl
38+
{
39+
namespace primary_interface
40+
{
41+
enum class ReportLevel : int32_t
42+
{
43+
DEBUG = 0,
44+
INFO = 1,
45+
WARNING = 2,
46+
VIOLATION = 3,
47+
FAULT = 4,
48+
DEVL_DEBUG = 128,
49+
DEVL_INFO = 129,
50+
DEVL_WARNING = 130,
51+
DEVL_VIOLATION = 131,
52+
DEVL_FAULT = 132
53+
};
54+
55+
struct ErrorCode
56+
{
57+
int32_t message_code{-1};
58+
int32_t message_argument{-1};
59+
ReportLevel report_level{ReportLevel::DEBUG};
60+
uint8_t data_type{0};
61+
uint32_t data{0};
62+
std::string text;
63+
uint64_t timestamp{0};
64+
std::string to_string;
65+
};
66+
67+
/*!
68+
* \brief The ErrorCodeMessage class handles the error code messages sent via the primary UR interface.
69+
*/
70+
class ErrorCodeMessage : public RobotMessage
71+
{
72+
public:
73+
ErrorCodeMessage() = delete;
74+
/*!
75+
* \brief Creates a new ErrorCodeMessage object to be filled from a package.
76+
*
77+
* \param timestamp Timestamp of the package
78+
* \param source The package's source
79+
*/
80+
ErrorCodeMessage(uint64_t timestamp, int8_t source)
81+
: RobotMessage(timestamp, source, RobotMessagePackageType::ROBOT_MESSAGE_ERROR_CODE)
82+
{
83+
}
84+
virtual ~ErrorCodeMessage() = default;
85+
86+
/*!
87+
* \brief Sets the attributes of the package by parsing a serialized representation of the
88+
* package.
89+
*
90+
* \param bp A parser containing a serialized text of the package
91+
*
92+
* \returns True, if the package was parsed successfully, false otherwise
93+
*/
94+
virtual bool parseWith(comm::BinParser& bp);
95+
96+
/*!
97+
* \brief Consume this package with a specific consumer.
98+
*
99+
* \param consumer Placeholder for the consumer calling this
100+
*
101+
* \returns true on success
102+
*/
103+
virtual bool consumeWith(AbstractPrimaryConsumer& consumer);
104+
105+
/*!
106+
* \brief Produces a human readable representation of the package object.
107+
*
108+
* \returns A string representing the object
109+
*/
110+
virtual std::string toString() const;
111+
112+
int32_t message_code_;
113+
int32_t message_argument_;
114+
ReportLevel report_level_;
115+
uint8_t data_type_;
116+
uint32_t data_;
117+
std::string text_;
118+
};
119+
} // namespace primary_interface
120+
} // namespace urcl
121+
122+
#endif // ifndef UR_CLIENT_LIBRARY_PRIMARY_TEXT_MESSAGE_H_INCLUDED

‎include/ur_client_library/ur/error_code_client.h‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
#include <memory>
2+
3+
#include "ur_client_library/comm/stream.h"
4+
#include <ur_client_library/comm/pipeline.h>
5+
#include "ur_client_library/comm/producer.h"
6+
#include "ur_client_library/primary/primary_package.h"
7+
#include <ur_client_library/primary/primary_parser.h>
8+
#include <ur_client_library/ur/error_code_reader.h>
9+
namespace urcl
10+
{
11+
class ErrorCodeClient
12+
{
13+
public:
14+
ErrorCodeClient() = delete;
15+
ErrorCodeClient(comm::URStream<primary_interface::PrimaryPackage>& stream, comm::INotifier& notifier,
16+
primary_interface::PrimaryParser& parser);
17+
~ErrorCodeClient();
18+
19+
void start();
20+
21+
std::deque<urcl::primary_interface::ErrorCode> getErrorCodes();
22+
23+
private:
24+
comm::URStream<primary_interface::PrimaryPackage>& stream_;
25+
urcl::ErrorCodeReader consumer_;
26+
primary_interface::PrimaryParser parser_;
27+
comm::URProducer<primary_interface::PrimaryPackage> prod_;
28+
comm::Pipeline<primary_interface::PrimaryPackage> pipeline_;
29+
};
30+
}

‎include/ur_client_library/ur/error_code_reader.h‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
#ifndef UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED
2+
#define UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED
3+
4+
#include <queue>
5+
#include <mutex>
6+
#include <ur_client_library/comm/pipeline.h>
7+
8+
#include <ur_client_library/primary/robot_message/error_code_message.h>
9+
10+
namespace urcl
11+
{
12+
/*!
13+
* \brief The ErrorCodeReader class consumes primary packages ignoring all but RobotCommMessage
14+
* packages to retrieve the latest code reported by the robot.
15+
*/
16+
class ErrorCodeReader : public comm::IConsumer<primary_interface::PrimaryPackage>
17+
{
18+
public:
19+
virtual ~ErrorCodeReader() = default;
20+
21+
/*!
22+
* \brief Empty setup function, as no setup is needed.
23+
*/
24+
virtual void setupConsumer()
25+
{
26+
}
27+
/*!
28+
* \brief Tears down the consumer.
29+
*/
30+
virtual void teardownConsumer()
31+
{
32+
}
33+
/*!
34+
* \brief Stops the consumer.
35+
*/
36+
virtual void stopConsumer()
37+
{
38+
}
39+
/*!
40+
* \brief Handles timeouts.
41+
*/
42+
virtual void onTimeout()
43+
{
44+
}
45+
46+
/*!
47+
* \brief Consumes a package and pushes it into a queue if it is an ErrorCodeMessage package.
48+
*
49+
* \param product The package to consume
50+
*
51+
* \returns True, if the package was consumed correctly
52+
*/
53+
virtual bool consume(std::shared_ptr<primary_interface::PrimaryPackage> product);
54+
55+
/*!
56+
* \brief Retrieves a list of error codes from the queue if there are any.
57+
*
58+
* \return A list of error codes
59+
*/
60+
std::deque<primary_interface::ErrorCode> getErrorCodesFromQueue();
61+
private:
62+
std::deque<primary_interface::ErrorCode> queue_;
63+
std::mutex queue_mutex_;
64+
65+
};
66+
} // namespace urcl
67+
68+
#endif // ifndef UR_CLIENT_LIBRARY_UR_ERROR_CODE_READER_H_INCLUDED

‎include/ur_client_library/ur/ur_driver.h‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
#include "ur_client_library/control/script_command_interface.h"
3737
#include "ur_client_library/control/script_sender.h"
3838
#include "ur_client_library/ur/tool_communication.h"
39+
#include "ur_client_library/ur/error_code_client.h"
3940
#include "ur_client_library/ur/version_information.h"
4041
#include "ur_client_library/ur/robot_receive_timeout.h"
4142
#include "ur_client_library/primary/robot_message/version_message.h"
@@ -409,6 +410,14 @@ class UrDriver
409410
*/
410411
bool checkCalibration(const std::string& checksum);
411412

413+
/*!
414+
* \brief Retrieves error codes ErrorCodeClient.
415+
*
416+
* \returns list of error codes
417+
*
418+
*/
419+
std::deque<urcl::primary_interface::ErrorCode> getErrorCodes();
420+
412421
/*!
413422
* \brief Getter for the RTDE writer used to write to the robot's RTDE interface.
414423
*
@@ -517,6 +526,12 @@ class UrDriver
517526
void resetRTDEClient(const std::string& output_recipe_filename, const std::string& input_recipe_filename,
518527
double target_frequency = 0.0);
519528

529+
/*!
530+
* \brief Starts the error code client
531+
*/
532+
void startErrorCodeClientCommunication();
533+
534+
520535
private:
521536
static std::string readScriptFile(const std::string& filename);
522537
/*!
@@ -533,6 +548,8 @@ class UrDriver
533548

534549
comm::INotifier notifier_;
535550
std::unique_ptr<rtde_interface::RTDEClient> rtde_client_;
551+
comm::INotifier error_code_notifier_;
552+
std::unique_ptr<ErrorCodeClient> error_code_client_;
536553
std::unique_ptr<control::ReverseInterface> reverse_interface_;
537554
std::unique_ptr<control::TrajectoryPointInterface> trajectory_interface_;
538555
std::unique_ptr<control::ScriptCommandInterface> script_command_interface_;

‎src/primary/robot_message/error_code_message.cpp‎

Lines changed: 70 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,70 @@
1+
// Below code is copied from a branch on the Universal_Robot_Client_Library
2+
// with some modification. Link to branch:
3+
// https://github.com/UniversalRobots/Universal_Robots_Client_Library/tree/improve_primary_interface
4+
//
5+
// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*-
6+
7+
// -- BEGIN LICENSE BLOCK ----------------------------------------------
8+
// Copyright 2019 FZI Forschungszentrum Informatik (ur_robot_driver)
9+
// Copyright 2017, 2018 Simon Rasmussen (refactor)
10+
//
11+
// Copyright 2015, 2016 Thomas Timm Andersen (original version)
12+
//
13+
// Licensed under the Apache License, Version 2.0 (the "License");
14+
// you may not use this file except in compliance with the License.
15+
// You may obtain a copy of the License at
16+
//
17+
// http://www.apache.org/licenses/LICENSE-2.0
18+
//
19+
// Unless required by applicable law or agreed to in writing, software
20+
// distributed under the License is distributed on an "AS IS" BASIS,
21+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
22+
// See the License for the specific language governing permissions and
23+
// limitations under the License.
24+
// -- END LICENSE BLOCK ------------------------------------------------
25+
26+
//----------------------------------------------------------------------
27+
/*!\file
28+
*
29+
* \author Felix Exner [email protected]
30+
* \date 2020-04-30
31+
*
32+
*/
33+
//----------------------------------------------------------------------
34+
35+
#include "ur_client_library/log.h"
36+
#include "ur_client_library/primary/robot_message/error_code_message.h"
37+
#include "ur_client_library/primary/abstract_primary_consumer.h"
38+
39+
namespace urcl
40+
{
41+
namespace primary_interface
42+
{
43+
bool ErrorCodeMessage::parseWith(comm::BinParser& bp)
44+
{
45+
bp.parse(message_code_);
46+
bp.parse(message_argument_);
47+
int32_t report_level;
48+
bp.parse(report_level);
49+
report_level_ = static_cast<ReportLevel>(report_level);
50+
bp.parse(data_type_);
51+
bp.parse(data_);
52+
bp.parseRemainder(text_);
53+
54+
return true; // not really possible to check dynamic size packets
55+
}
56+
57+
bool ErrorCodeMessage::consumeWith(AbstractPrimaryConsumer& consumer)
58+
{
59+
return consumer.consume(*this);
60+
}
61+
62+
std::string ErrorCodeMessage::toString() const
63+
{
64+
std::stringstream ss;
65+
ss << "C" << message_code_ << "A" << message_argument_;
66+
return ss.str();
67+
}
68+
69+
} // namespace primary_interface
70+
} // namespace urcl

0 commit comments

Comments
 (0)