Commit 7c44cf08 authored by Lorenz Halt's avatar Lorenz Halt 🔀
Browse files

ros package working

parent 144aa965
......@@ -90,7 +90,7 @@ install(TARGETS
catkin_install_python(PROGRAMS
scripts/ilc_rosproxy.py
py_src/ilc/scripts/ilc_rosproxy
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
......
......@@ -97,13 +97,16 @@ protected:
auto closest_ff = feedforward_.upper_bound(tick_ms); // uses upper bound to avoid negative ticks and not reusing the 'last' values if there is nothing in file
if (closest_ff != feedforward_.end()) { // fitting tick found
pi_info("[Coordinate]: [{}]", coordinate);
string coordinate_str = coordinate;
coordinate_str.erase(0, coordinate_str.find_first_of("/")+1); // delete id from string
auto it = closest_ff->second.find(coordinate_str);
if (it == closest_ff->second.end())
{
pi_error("Although closest tick was found, the respective coordinate was not found. This is very odd and probably points to a corrupted input file.");
pi_error("Although closest tick was found, the respective coordinate was not found. This probably points to a corrupted input file.");
pi_error("Coordinate: {}", coordinate_str);
// pi_error("Candidates: {}" closest_ff->second);
} else {
auto res_ff = res + it->second;
pi_debug("Tick: {} Coordinate: {}\nres: {}, ff: {}, res+ff: {}", tick_ms,
......
......@@ -36,7 +36,7 @@
<!-- republish the joint velocities from a C++ node (https://gitlab.cc-asp.fraunhofer.de/pitasc/pitasc/-/issues/367) -->
<node name="joint_vel_republisher" pkg="microsim" type="joint_vel_republisher_node" output="screen" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5_robot.urdf.xacro'" />
<param name="robot_description" command="$(find xacro)/xacro --inorder '$(find ur_description)/urdf/ur5.xacro'" />
<!-- <node name="robot_model" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param robot_description -model robot
......@@ -65,7 +65,13 @@
<!-- <node name="bin" pkg="gazebo_ros" type="spawn_model"
args="-urdf -param box/robot_description -model bin -y 0.6 -z 0.05 " respawn="false" output="screen" /> -->
<node pkg="robot_state_publisher" type="robot_state_publisher" name="box_state_publisher" ns="box"/>
<!-- <node pkg="robot_state_publisher" type="robot_state_publisher" name="box_state_publisher"/> -->
<node name="ee_link_publisher" pkg="tf" type="static_transform_publisher"
args="0 0 0 1 0 0 0 tool0 ee_link 100"/>
<node name="world_publisher" pkg="tf" type="static_transform_publisher"
args="0 0 0 1 0 0 0 base world 100"/>
<node name="start_pos_publisher" pkg="tf" type="static_transform_publisher"
args="-0.05 0.6 0.5 1 0 0 0 world start_position 100"/>
......@@ -90,7 +96,7 @@
args="-0.05 0.6 -0.02 1 0 0 0 world target2 100"/>
<!-- <node name="pitasc_http_server" pkg="pitasc_http" type="http_server.py" /> -->
<node name="ilc_rosproxy" pkg="ilc" type="ilc_rosproxy.py" output="screen" >
<node name="ilc_rosproxy" pkg="ilc" type="ilc_rosproxy" output="screen" >
<param name="T" value="8" />
<param name="folder" value="$(find ilc)/logs/ilc_proxy" />
</node>
......
......@@ -6,7 +6,7 @@
<maintainer email="lorenz.halt@ipa.fraunhofer.de">ipa-lth</maintainer>
<license>Proprietary</license>
<license>BSD</license>
<url type="website">http://www.ipa.fraunhofer.de/</url>
......
#!/usr/bin/env python
from __future__ import division
import numpy as np
import os
......@@ -51,7 +53,7 @@ def write(file, data, header):
delimiter =",")
def read(file):
#print("Reading from file: '%s'"%(file))
print("Reading from file: '%s'"%(file))
with open(file, 'r') as f:
header_str = f.readline().strip()
data = np.genfromtxt(file,
......
......@@ -4,7 +4,7 @@ from __future__ import print_function
from ilc.srv import *
import rospy
import numpy as np
import ilc_lib
import ilc.ilc_lib as ilc_lib
import os
import re
import rospkg
......@@ -51,7 +51,7 @@ class Ilc_serviceproxy:
rospack = rospkg.RosPack()
name = name.replace(matchObj.group(), "{}".format(rospack.get_path(matchObj.group(2))))
#print("Decoded Name: {} -> {}".format(filename, name))
print("Decoded Name: {} -> {}".format(filename, name))
return name
......@@ -86,8 +86,13 @@ class Ilc_serviceproxy:
def callback_ilc_calc(self, req):
try:
print("Callback ILC calc - Z88")
path = self.decode_filename(req.filename_error)
print("Callback ILC calc - Z92")
print(path)
print("Callback ILC calc - 95")
### Read Error data
data_org, header = ilc_lib.read(path)
message = self.ilc_calc(req, data_org, header)
......@@ -95,8 +100,9 @@ class Ilc_serviceproxy:
return TriggerILCResponse(success=True, message=message)
except Exception as e:
print("except")
print("exception")
message = str(e)
print(message)
return TriggerILCResponse(success=False, message=message)
......
#!/usr/bin/env python
import rospy
from ilc_lib import ilc_rosproxy
from ilc import ilc_roswrapper
if __name__ == '__main__':
rospy.init_node('ilc_proxy')
ilc_rosproxy.Ilc_serviceproxy()
rospy.init_node('ilc_rosproxy')
ilc_roswrapper.Ilc_serviceproxy()
print("ILC: Ready to serve.")
rospy.spin()
......@@ -11,12 +11,12 @@ from pitasc_install_utils.custom_build_pyc import build_pyc_include
d = generate_distutils_setup(
packages=['ilc'],
package_dir={'': 'scripts'}
package_dir={'': 'py_src'}
)
setup(ext_modules = cythonize(
build_pyc_include.get_py_list([
'scripts/ilc_lib/'
'py_src/ilc/'
]),
build_dir='/tmp/cython_build/',
language_level = int(os.environ['ROS_PYTHON_VERSION'])),
......
#include "controller_ilc.hpp"
#include "ilc/controller_ilc.hpp"
#include "cppitasc/controllers/p_controller.hpp"
#include "cppitasc/controllers/pd_controller.hpp"
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment