Skip to content

Commit 6fe6403

Browse files
LuziaKnLuziaKn
committedSep 8, 2023
add basics for navigating to goal
1 parent 94702ca commit 6fe6403

22 files changed

+2431
-5
lines changed
 

‎README.md

+4
Original file line numberDiff line numberDiff line change
@@ -23,5 +23,9 @@ This generates a folder named 'AlbertFORCESNLPsolver' in python_solver_generatio
2323

2424
[ToDo: Extension for Acados]
2525

26+
## Test in Simulation
27+
```
28+
roslaunch albert_gazebo albert_gazebo_navigation.launch panda_control_mode:=velocity localization:=amcl world:=AH_store-2
29+
```
2630

2731

‎base_mpc/mpc_pkg/CMakeLists.txt

+234
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,234 @@
1+
cmake_minimum_required(VERSION 3.0.2)
2+
project(mpc_pkg)
3+
4+
find_package(Eigen3 REQUIRED)
5+
find_package(decomp_util REQUIRED)
6+
7+
## Compile as C++11, supported in ROS Kinetic and newer
8+
# add_compile_options(-std=c++11)
9+
10+
## Find catkin macros and libraries
11+
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
12+
## is used, also find other catkin packages
13+
find_package(catkin REQUIRED COMPONENTS
14+
geometry_msgs
15+
jsk_recognition_msgs
16+
nav_msgs
17+
laser_geometry
18+
pcl_msgs
19+
roscpp
20+
rospy
21+
sensor_msgs
22+
std_msgs
23+
tf2_ros
24+
tf
25+
visualization_msgs
26+
message_generation
27+
)
28+
29+
## System dependencies are found with CMake's conventions
30+
# find_package(Boost REQUIRED COMPONENTS system)
31+
32+
33+
## Uncomment this if the package has a setup.py. This macro ensures
34+
## modules and global scripts declared therein get installed
35+
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
36+
catkin_python_setup()
37+
38+
################################################
39+
## Declare ROS messages, services and actions ##
40+
################################################
41+
42+
## To declare and build messages, services or actions from within this
43+
## package, follow these steps:
44+
## * Let MSG_DEP_SET be the set of packages whose message types you use in
45+
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
46+
## * In the file package.xml:
47+
## * add a build_depend tag for "message_generation"
48+
## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
49+
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
50+
## but can be declared for certainty nonetheless:
51+
## * add a exec_depend tag for "message_runtime"
52+
## * In this file (CMakeLists.txt):
53+
## * add "message_generation" and every package in MSG_DEP_SET to
54+
## find_package(catkin REQUIRED COMPONENTS ...)
55+
## * add "message_runtime" and every package in MSG_DEP_SET to
56+
## catkin_package(CATKIN_DEPENDS ...)
57+
## * uncomment the add_*_files sections below as needed
58+
## and list every .msg/.srv/.action file to be processed
59+
## * uncomment the generate_messages entry below
60+
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
61+
62+
## Generate messages in the 'msg' folder
63+
# add_message_files(
64+
# FILES
65+
# Message1.msg
66+
# Message2.msg
67+
# )
68+
69+
## Generate services in the 'srv' folder
70+
add_service_files(
71+
FILES
72+
GoalInfo.srv
73+
PoseInfo.srv
74+
TwistInfo.srv
75+
MpcConstraint.srv
76+
)
77+
78+
## Generate actions in the 'action' folder
79+
# add_action_files(
80+
# FILES
81+
# Action1.action
82+
# Action2.action
83+
# )
84+
85+
## Generate added messages and services with any dependencies listed here
86+
generate_messages(
87+
DEPENDENCIES
88+
geometry_msgs
89+
std_msgs
90+
jsk_recognition_msgs# nav_msgs# pcl_msgs# sensor_msgs# std_msgs# visualization_msgs
91+
)
92+
93+
################################################
94+
## Declare ROS dynamic reconfigure parameters ##
95+
################################################
96+
97+
## To declare and build dynamic reconfigure parameters within this
98+
## package, follow these steps:
99+
## * In the file package.xml:
100+
## * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
101+
## * In this file (CMakeLists.txt):
102+
## * add "dynamic_reconfigure" to
103+
## find_package(catkin REQUIRED COMPONENTS ...)
104+
## * uncomment the "generate_dynamic_reconfigure_options" section below
105+
## and list every .cfg file to be processed
106+
107+
## Generate dynamic reconfigure parameters in the 'cfg' folder
108+
# generate_dynamic_reconfigure_options(
109+
# cfg/DynReconf1.cfg
110+
# cfg/DynReconf2.cfg
111+
# )
112+
113+
###################################
114+
## catkin specific configuration ##
115+
###################################
116+
## The catkin_package macro generates cmake config files for your package
117+
## Declare things to be passed to dependent projects
118+
## INCLUDE_DIRS: uncomment this if your package contains header files
119+
## LIBRARIES: libraries you create in this project that dependent projects also need
120+
## CATKIN_DEPENDS: catkin_packages dependent projects also need
121+
## DEPENDS: system dependencies of this project that dependent projects also need
122+
catkin_package(
123+
INCLUDE_DIRS include
124+
LIBRARIES mpc_pkg
125+
CATKIN_DEPENDS geometry_msgs jsk_recognition_msgs nav_msgs pcl_msgs roscpp rospy sensor_msgs std_msgs tf2_ros visualization_msgs laser_geometry message_runtime
126+
# DEPENDS system_lib
127+
)
128+
129+
130+
###########
131+
## Build ##
132+
###########
133+
134+
## Specify additional locations of header files
135+
## Your package locations should be listed before other locations
136+
include_directories(
137+
include
138+
${EIGEN3_INCLUDE_DIRS}
139+
${Boost_INCLUDE_DIRS}
140+
${catkin_INCLUDE_DIRS}
141+
${DECOMP_UTIL_INCLUDE_DIRS}
142+
)
143+
144+
## Declare a C++ library
145+
add_library(sfc_generate_cls_lib
146+
src/SFC_generate_cls.cpp
147+
src/logger.cpp
148+
)
149+
150+
## Add cmake target dependencies of the library
151+
## as an example, code may need to be generated before libraries
152+
## either from message generation or dynamic reconfigure
153+
# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
154+
155+
## Declare a C++ executable
156+
## With catkin_make all packages are built within a single CMake context
157+
## The recommended prefix ensures that target names across packages don't collide
158+
add_executable(SFC_generate_node src/SFC_generate_node.cpp)
159+
target_link_libraries(SFC_generate_node
160+
sfc_generate_cls_lib
161+
${Boost_LIBRARIES}
162+
${catkin_LIBRARIES}
163+
${DECOMP_UTIL_LIBRARIES}
164+
)
165+
166+
## Rename C++ executable without prefix
167+
## The above recommended prefix causes long target names, the following renames the
168+
## target back to the shorter version for ease of user use
169+
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
170+
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
171+
172+
## Add cmake target dependencies of the executable
173+
## same as for the library above
174+
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
175+
176+
## Specify libraries to link a library or executable target against
177+
# target_link_libraries(${PROJECT_NAME}_node
178+
# ${catkin_LIBRARIES}
179+
# )
180+
181+
#############
182+
## Install ##
183+
#############
184+
185+
# all install targets should use catkin DESTINATION variables
186+
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
187+
188+
## Mark executable scripts (Python etc.) for installation
189+
## in contrast to setup.py, you can choose the destination
190+
# catkin_install_python(PROGRAMS
191+
# scripts/my_python_script
192+
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
193+
# )
194+
195+
## Mark executables for installation
196+
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
197+
# install(TARGETS ${PROJECT_NAME}_node
198+
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
199+
# )
200+
201+
## Mark libraries for installation
202+
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
203+
# install(TARGETS ${PROJECT_NAME}
204+
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
205+
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
206+
# RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
207+
# )
208+
209+
## Mark cpp header files for installation
210+
# install(DIRECTORY include/${PROJECT_NAME}/
211+
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
212+
# FILES_MATCHING PATTERN "*.h"
213+
# PATTERN ".svn" EXCLUDE
214+
# )
215+
216+
## Mark other files for installation (e.g. launch and bag files, etc.)
217+
catkin_install_python(PROGRAMS
218+
src/goal_receiver_node.py
219+
src/mpc_node.py
220+
DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
221+
)
222+
223+
#############
224+
## Testing ##
225+
#############
226+
227+
## Add gtest based cpp test target and link libraries
228+
# catkin_add_gtest(${PROJECT_NAME}-test test/test_mpc_pkg.cpp)
229+
# if(TARGET ${PROJECT_NAME}-test)
230+
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
231+
# endif()
232+
233+
## Add folders to be run by python nosetests
234+
# catkin_add_nosetests(test)

‎base_mpc/mpc_pkg/__init__.py

Whitespace-only changes.

‎base_mpc/mpc_pkg/include/mpc_pkg/SFC_generate_cls.h

+43
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
#ifndef SFC_GENERATE_NODE
2+
#define SFC_GENERATE_NODE
3+
4+
#include <ros/ros.h>
5+
#include <decomp_util/ellipsoid_decomp.h>
6+
#include <decomp_geometry/geometric_utils.h>
7+
8+
#include <boost/geometry.hpp>
9+
#include <boost/geometry/geometries/point_xy.hpp>
10+
#include <boost/geometry/geometries/polygon.hpp>
11+
12+
#include "mpc_pkg/logger.h"
13+
#include "mpc_pkg/MpcConstraint.h"
14+
#include <nav_msgs/OccupancyGrid.h>
15+
#include <visualization_msgs/Marker.h>
16+
#include <gazebo_msgs/ModelStates.h>
17+
#include <sensor_msgs/LaserScan.h>
18+
#include <tf/transform_listener.h>
19+
20+
class SFCGenerateNode
21+
{
22+
public:
23+
SFCGenerateNode();
24+
void scan_multiCB(const sensor_msgs::LaserScan::ConstPtr& data);
25+
bool GetMpcConstraintsCB(mpc_pkg::MpcConstraint::Request & req, mpc_pkg::MpcConstraint::Response & res);
26+
27+
private:
28+
ros::NodeHandle nh;
29+
30+
ros::Subscriber scan_multi_sub;
31+
32+
ros::Publisher polygon_pub;
33+
ros::Publisher point_cloud_pub;
34+
ros::ServiceServer mpc_constraints_service;
35+
36+
vec_Vec2f path;
37+
vec_Vec2f obs;
38+
Logger logger;
39+
40+
41+
};
42+
43+
#endif

‎base_mpc/mpc_pkg/include/mpc_pkg/logger.h

+57
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,57 @@
1+
#ifndef LOGGER
2+
#define LOGGER
3+
4+
# include <iostream>
5+
# include <ros/ros.h>
6+
# include <string>
7+
8+
class Logger
9+
{
10+
public:
11+
Logger(std::string prefix, int log_frequence, bool enable_logging = true);
12+
void log(const std::string& msg, int type = 0);
13+
void get_enable(const bool& a);
14+
15+
private:
16+
std::string prefix;
17+
int log_frequence;
18+
bool enable_logging;
19+
int log_counts;
20+
};
21+
22+
namespace pc
23+
{
24+
enum PRINT_COLOR
25+
{
26+
BLACK,
27+
RED,
28+
GREEN,
29+
YELLOW,
30+
BLUE,
31+
MAGENTA,
32+
CYAN,
33+
WHITE,
34+
ENDCOLOR
35+
};
36+
37+
std::ostream& operator<<(std::ostream& os, PRINT_COLOR c);
38+
} //namespace pc
39+
40+
#define ROS_BLACK_STREAM(x) ROS_INFO_STREAM(pc::BLACK << x << pc::ENDCOLOR)
41+
#define ROS_RED_STREAM(x) ROS_INFO_STREAM(pc::RED << x << pc::ENDCOLOR)
42+
#define ROS_GREEN_STREAM(x) ROS_INFO_STREAM(pc::GREEN << x << pc::ENDCOLOR)
43+
#define ROS_YELLOW_STREAM(x) ROS_INFO_STREAM(pc::YELLOW << x << pc::ENDCOLOR)
44+
#define ROS_BLUE_STREAM(x) ROS_INFO_STREAM(pc::BLUE << x << pc::ENDCOLOR)
45+
#define ROS_MAGENTA_STREAM(x) ROS_INFO_STREAM(pc::MAGENTA << x << pc::ENDCOLOR)
46+
#define ROS_CYAN_STREAM(x) ROS_INFO_STREAM(pc::CYAN << x << pc::ENDCOLOR)
47+
48+
#define ROS_BLACK_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::BLACK << x << pc::ENDCOLOR)
49+
#define ROS_RED_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::RED << x << pc::ENDCOLOR)
50+
#define ROS_GREEN_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::GREEN << x << pc::ENDCOLOR)
51+
#define ROS_YELLOW_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::YELLOW << x << pc::ENDCOLOR)
52+
#define ROS_BLUE_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::BLUE << x << pc::ENDCOLOR)
53+
#define ROS_MAGENTA_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::MAGENTA << x << pc::ENDCOLOR)
54+
#define ROS_CYAN_STREAM_COND(c, x) ROS_INFO_STREAM_COND(c, pc::CYAN << x << pc::ENDCOLOR)
55+
56+
57+
#endif

‎base_mpc/mpc_pkg/launch/nav2goal.launch

+13
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
<launch>
2+
<param name="goal_receiver_enable_logging" value="true"/>
3+
4+
<node pkg="mpc_pkg" type="goal_receiver_node.py" name="goal_receiver_node" output="screen"/>
5+
6+
7+
<param name="SFC_generate_enable_logging" value="false"/>
8+
<param name="mpc_enable_logging" value="true"/>
9+
10+
<!--><node pkg="mpc_pkg" type="SFC_generate_node" name="SFC_generate_node" output="screen"/>-->
11+
<node pkg="mpc_pkg" type="mpc_node.py" name="mpc_node" output="screen"/>
12+
13+
</launch>

‎base_mpc/mpc_pkg/package.xml

+112
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,112 @@
1+
<?xml version="1.0"?>
2+
<package format="2">
3+
<name>mpc_pkg</name>
4+
<version>0.0.0</version>
5+
<description>The mpc_pkg package</description>
6+
7+
<!-- One maintainer tag required, multiple allowed, one person per tag -->
8+
<!-- Example: -->
9+
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10+
<maintainer email="luzia@todo.todo">luzia</maintainer>
11+
12+
13+
<!-- One license tag required, multiple allowed, one license per tag -->
14+
<!-- Commonly used license strings: -->
15+
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16+
<license>TODO</license>
17+
18+
19+
<!-- Url tags are optional, but multiple are allowed, one per tag -->
20+
<!-- Optional attribute type can be: website, bugtracker, or repository -->
21+
<!-- Example: -->
22+
<!-- <url type="website">http://wiki.ros.org/mpc_pkg</url> -->
23+
24+
25+
<!-- Author tags are optional, multiple are allowed, one per tag -->
26+
<!-- Authors do not have to be maintainers, but could be -->
27+
<!-- Example: -->
28+
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
29+
30+
31+
<!-- The *depend tags are used to specify dependencies -->
32+
<!-- Dependencies can be catkin packages or system dependencies -->
33+
<!-- Examples: -->
34+
<!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
35+
<!-- <depend>roscpp</depend> -->
36+
<!-- Note that this is equivalent to the following: -->
37+
<!-- <build_depend>roscpp</build_depend> -->
38+
<!-- <exec_depend>roscpp</exec_depend> -->
39+
<!-- Use build_depend for packages you need at compile time: -->
40+
<!-- <build_depend>message_generation</build_depend> -->
41+
<!-- Use build_export_depend for packages you need in order to build against this package: -->
42+
<!-- <build_export_depend>message_generation</build_export_depend> -->
43+
<!-- Use buildtool_depend for build tool packages: -->
44+
<!-- <buildtool_depend>catkin</buildtool_depend> -->
45+
<!-- Use exec_depend for packages you need at runtime: -->
46+
<!-- <exec_depend>message_runtime</exec_depend> -->
47+
<!-- Use test_depend for packages you need only for testing: -->
48+
<!-- <test_depend>gtest</test_depend> -->
49+
<!-- Use doc_depend for packages you need only for building documentation: -->
50+
<!-- <doc_depend>doxygen</doc_depend> -->
51+
<buildtool_depend>catkin</buildtool_depend>
52+
53+
54+
<!-- The export tag contains other, unspecified, tags -->
55+
56+
<!-- Other tools can request additional information be placed here -->
57+
<buildtool_depend>catkin</buildtool_depend>
58+
<build_depend>geometry_msgs</build_depend>
59+
<build_depend>jsk_recognition_msgs</build_depend>
60+
<build_depend>lmpcc_msgs</build_depend>
61+
<build_depend>nav_msgs</build_depend>
62+
<build_depend>laser_geometry</build_depend>
63+
<build_depend>pcl_msgs</build_depend>
64+
<build_depend>roscpp</build_depend>
65+
<build_depend>rospy</build_depend>
66+
<build_depend>sensor_msgs</build_depend>
67+
<build_depend>spencer_tracking_msgs</build_depend>
68+
<build_depend>std_msgs</build_depend>
69+
<build_depend>tf2_ros</build_depend>
70+
<build_depend>tf</build_depend>
71+
<build_depend>visualization_msgs</build_depend>
72+
<build_depend>obstacle_detector</build_depend>
73+
<build_depend>jsk_rviz_plugins</build_depend>
74+
<build_export_depend>geometry_msgs</build_export_depend>
75+
<build_export_depend>jsk_recognition_msgs</build_export_depend>
76+
<build_export_depend>lmpcc_msgs</build_export_depend>
77+
<build_export_depend>nav_msgs</build_export_depend>
78+
<build_export_depend>laser_geometry</build_export_depend>
79+
<build_export_depend>pcl_msgs</build_export_depend>
80+
<build_export_depend>roscpp</build_export_depend>
81+
<build_export_depend>rospy</build_export_depend>
82+
<build_export_depend>sensor_msgs</build_export_depend>
83+
<build_export_depend>spencer_tracking_msgs</build_export_depend>
84+
<build_export_depend>std_msgs</build_export_depend>
85+
<build_export_depend>tf2_ros</build_export_depend>
86+
<build_export_depend>tf</build_export_depend>
87+
<build_export_depend>visualization_msgs</build_export_depend>
88+
<build_export_depend>obstacle_detector</build_export_depend>
89+
<build_export_depend>jsk_rviz_plugins</build_export_depend>
90+
<exec_depend>geometry_msgs</exec_depend>
91+
<exec_depend>jsk_recognition_msgs</exec_depend>
92+
<exec_depend>lmpcc_msgs</exec_depend>
93+
<exec_depend>nav_msgs</exec_depend>
94+
<exec_depend>laser_geometry</exec_depend>
95+
<exec_depend>pcl_msgs</exec_depend>
96+
<exec_depend>roscpp</exec_depend>
97+
<exec_depend>rospy</exec_depend>
98+
<exec_depend>sensor_msgs</exec_depend>
99+
<exec_depend>spencer_tracking_msgs</exec_depend>
100+
<exec_depend>std_msgs</exec_depend>
101+
<exec_depend>tf2_ros</exec_depend>
102+
<exec_depend>tf</exec_depend>
103+
<exec_depend>visualization_msgs</exec_depend>
104+
<exec_depend>obstacle_detector</exec_depend>
105+
<exec_depend>jsk_rviz_plugins</exec_depend>
106+
107+
<build_depend>message_generation</build_depend>
108+
<exec_depend>message_runtime</exec_depend>
109+
110+
<export>
111+
</export>
112+
</package>

‎base_mpc/mpc_pkg/scripts/ROS_visualization/__init__.py

Whitespace-only changes.

‎base_mpc/mpc_pkg/scripts/ROS_visualization/logger.py

+38
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,38 @@
1+
import rospy
2+
from termcolor import colored
3+
4+
5+
class Logger():
6+
def __init__(self, prefix, log_frequence, enable_logging=True):
7+
self.prefix = prefix
8+
self.log_frequence = log_frequence
9+
self.enable_logging = enable_logging
10+
self.log_counts = 0
11+
para_name = self.prefix + "_enable_logging"
12+
self.enable_logging = rospy.get_param(para_name, True)
13+
14+
def log(self, msg, type=0):
15+
"""
16+
type:
17+
-1 -> error
18+
0 -> normal
19+
1 -> important
20+
"""
21+
22+
if self.enable_logging:
23+
prefix_temp = self.prefix + ": "
24+
if type == 0 and self.log_counts % self.log_frequence == 0:
25+
26+
print(prefix_temp, msg)
27+
28+
elif type == 1:
29+
30+
print(colored(prefix_temp, 'blue'))
31+
print(colored(msg, 'blue'))
32+
33+
if type == -1:
34+
print(colored(prefix_temp, 'red'))
35+
print(colored(msg, 'red'))
36+
37+
if msg == "end":
38+
self.log_counts += 1;

‎base_mpc/mpc_pkg/scripts/ROS_visualization/ros_visuals.py

+238
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,238 @@
1+
import rospy
2+
from visualization_msgs.msg import MarkerArray, Marker
3+
from geometry_msgs.msg import Pose, Point
4+
from copy import deepcopy
5+
6+
COLORS = [[153, 51, 102], [0, 255, 0], [254, 254, 0], [254, 0, 254], [0, 255, 255], [255, 153, 0]]
7+
8+
class ROSMarkerPublisher:
9+
10+
"""
11+
Collects ROS Markers
12+
"""
13+
14+
def __init__(self, topic, max_size):
15+
self.pub_ = rospy.Publisher(topic, MarkerArray, queue_size=1)
16+
self.marker_list_ = MarkerArray()
17+
18+
self.clear_all()
19+
20+
self.ros_markers_ = []
21+
22+
self.topic_ = topic
23+
self.max_size_ = max_size
24+
self.id_ = 0
25+
self.prev_id_ = 0
26+
27+
def clear_all(self):
28+
remove_marker = Marker()
29+
remove_marker.action = remove_marker.DELETEALL
30+
self.marker_list_.markers = []
31+
self.marker_list_.markers.append(remove_marker)
32+
self.pub_.publish(self.marker_list_)
33+
34+
def __del__(self):
35+
self.clear_all()
36+
37+
def publish(self):
38+
# print('DEBUG: Publishing markers')
39+
40+
remove_marker = Marker()
41+
remove_marker.action = remove_marker.DELETE
42+
43+
if self.prev_id_ > self.id_:
44+
for i in range(self.id_, self.prev_id_):
45+
remove_marker.id = i
46+
self.marker_list_.markers.append(deepcopy(remove_marker))
47+
48+
# Define stamp properties
49+
for marker in self.marker_list_.markers:
50+
marker.header.stamp = rospy.Time.now()
51+
52+
# Publish
53+
# print(self.marker_list_.markers)
54+
#print('Publishing {} markers'.format(len(self.marker_list_.markers)))
55+
self.pub_.publish(self.marker_list_)
56+
57+
# Reset
58+
self.marker_list_.markers = []
59+
60+
self.prev_id_ = deepcopy(self.id_)
61+
self.id_ = 0
62+
63+
def get_cube(self, frame_id="map"):
64+
new_marker = ROSMarker(1, frame_id, self)
65+
return self.get_point_marker(new_marker)
66+
67+
def get_sphere(self, frame_id="map"):
68+
new_marker = ROSMarker(2, frame_id, self)
69+
return self.get_point_marker(new_marker)
70+
71+
def get_cylinder(self, frame_id="map"):
72+
new_marker = ROSMarker(3, frame_id, self)
73+
return self.get_point_marker(new_marker)
74+
75+
def get_circle(self, frame_id="map"):
76+
new_marker = ROSMarker(3, frame_id, self)
77+
new_marker.set_scale(1., 1., 0.01)
78+
return self.get_point_marker(new_marker)
79+
80+
def get_line(self, frame_id="map"):
81+
new_marker = ROSLineMarker(frame_id, self)
82+
return self.get_point_marker(new_marker)
83+
84+
def get_point_marker(self, new_marker):
85+
self.ros_markers_.append(new_marker)
86+
return new_marker # Gives the option to modify it
87+
88+
def add_marker_(self, new_marker):
89+
self.marker_list_.markers.append(new_marker)
90+
91+
def get_id_(self):
92+
cur_id = deepcopy(self.id_)
93+
self.id_ += 1
94+
return cur_id
95+
96+
class ROSMarker:
97+
98+
def __init__(self, type, frame_id, ros_marker_publisher):
99+
self.marker_ = Marker()
100+
self.marker_.header.frame_id = frame_id
101+
self.type_ = type
102+
self.marker_.type = self.type_
103+
self.ros_marker_publisher_ = ros_marker_publisher
104+
105+
self.set_color(0)
106+
self.set_scale(1, 1, 1)
107+
108+
def set_scale(self, x, y, z):
109+
self.marker_.scale.x = x
110+
self.marker_.scale.y = y
111+
self.marker_.scale.z = z
112+
113+
def set_scale_all(self, all=1.0):
114+
self.marker_.scale.x = all
115+
self.marker_.scale.y = all
116+
self.marker_.scale.z = all
117+
118+
def set_color(self, int_val, alpha=1.0):
119+
red, green, blue = get_viridis_color(int_val)
120+
self.marker_.color.r = red
121+
self.marker_.color.g = green
122+
self.marker_.color.b = blue
123+
self.marker_.color.a = alpha
124+
125+
def set_lifetime(self, lifetime):
126+
self.marker_.lifetime = rospy.Time(lifetime)
127+
128+
def add_marker(self, pose):
129+
self.marker_.id = self.ros_marker_publisher_.get_id_()
130+
self.marker_.pose = pose
131+
self.marker_.pose.orientation.x = 0
132+
self.marker_.pose.orientation.y = 0
133+
self.marker_.pose.orientation.z = 0
134+
self.marker_.pose.orientation.w = 1
135+
self.ros_marker_publisher_.add_marker_(deepcopy(self.marker_)) # Note the deepcopy
136+
137+
class ROSLineMarker:
138+
139+
def __init__(self, frame_id, ros_marker_publisher):
140+
self.marker_ = Marker()
141+
self.marker_.header.frame_id = frame_id
142+
self.marker_.type = 5
143+
self.ros_marker_publisher_ = ros_marker_publisher
144+
145+
self.marker_.scale.x = 0.25
146+
self.marker_.pose.orientation.x = 0
147+
self.marker_.pose.orientation.y = 0
148+
self.marker_.pose.orientation.z = 0
149+
self.marker_.pose.orientation.w = 1
150+
self.set_color(1)
151+
152+
# Ax <= b (visualized)
153+
def add_constraint_line(self, A, b, length=10.0):
154+
a1 = A[0]
155+
a2 = A[1]
156+
b = float(b)
157+
p1 = Point()
158+
p2 = Point()
159+
if abs(a1) < 0.01 and abs(a2) >= 0.01:
160+
p1.x = -length
161+
p1.y = (b + a1 * length) / float(a2)
162+
p1.z = 0.
163+
164+
p2.x = length
165+
p2.y = (b - a1 * length) / float(a2)
166+
p2.z = 0.
167+
elif(abs(a1) >= 0.01):
168+
p1.y = -length
169+
p1.x = (b + a2 * length) / float(a1)
170+
p1.z = 0.
171+
172+
p2.y = length
173+
p2.x = (b - a2 * length) / float(a1)
174+
p2.z = 0.
175+
else:
176+
raise Exception("visualized constrained is zero")
177+
178+
p1.x = min(max(p1.x, -1e5), 1e5)
179+
p2.x = min(max(p2.x, -1e5), 1e5)
180+
p1.y = min(max(p1.y, -1e5), 1e5)
181+
p2.y = min(max(p2.y, -1e5), 1e5)
182+
183+
self.add_line(p1, p2)
184+
185+
def add_line_from_poses(self, pose_one, pose_two):
186+
p1 = Point()
187+
p1.x = pose_one.position.x
188+
p1.y = pose_one.position.y
189+
p1.z = pose_one.position.z
190+
191+
p2 = Point()
192+
p2.x = pose_two.position.x
193+
p2.y = pose_two.position.y
194+
p2.z = pose_two.position.z
195+
196+
self.add_line(p1, p2)
197+
198+
def add_line(self, point_one, point_two):
199+
self.marker_.id = self.ros_marker_publisher_.get_id_()
200+
201+
self.marker_.points.append(point_one)
202+
self.marker_.points.append(point_two)
203+
204+
self.ros_marker_publisher_.add_marker_(deepcopy(self.marker_))
205+
206+
self.marker_.points = []
207+
208+
def set_color(self, int_val, alpha=1.0):
209+
red, green, blue = get_viridis_color(int_val)
210+
self.marker_.color.r = red
211+
self.marker_.color.g = green
212+
self.marker_.color.b = blue
213+
self.marker_.color.a = alpha
214+
215+
def set_scale(self, thickness):
216+
self.marker_.scale.x = thickness
217+
218+
219+
def get_viridis_color(select):
220+
# Obtained from https://waldyrious.net/viridis-palette-generator/
221+
viridis_vals = [253, 231, 37, 234, 229, 26, 210, 226, 27, 186, 222, 40, 162, 218, 55, 139, 214, 70, 119, 209, 83, 99,
222+
203, 95, 80, 196, 106, 63, 188, 115, 49, 181, 123, 38, 173, 129, 33, 165, 133, 30, 157, 137, 31, 148, 140, 34, 140,
223+
141, 37, 131, 142, 41, 123, 142, 44, 115, 142, 47, 107, 142, 51, 98, 141, 56, 89, 140]
224+
225+
VIRIDIS_COLORS = 20
226+
select %= VIRIDIS_COLORS # only 20 values specified
227+
228+
# Invert the color range
229+
select = VIRIDIS_COLORS - 1 - select
230+
red = viridis_vals[select * 3 + 0]
231+
green = viridis_vals[select * 3 + 1]
232+
blue = viridis_vals[select * 3 + 2]
233+
234+
red /= 256.0
235+
green /= 256.0
236+
blue /= 256.0
237+
238+
return red, green, blue

‎base_mpc/mpc_pkg/setup.py

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
from distutils.core import setup
2+
from catkin_pkg.python_setup import generate_distutils_setup
3+
4+
d = generate_distutils_setup(
5+
packages=['ROS_visualization'],
6+
package_dir={'': 'scripts'}
7+
)
8+
9+
10+
11+
setup(**d)

‎base_mpc/mpc_pkg/src/SFC_generate_cls.cpp

+143
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,143 @@
1+
2+
3+
4+
#include <ros/ros.h>
5+
#include <cmath>
6+
#include <decomp_util/ellipsoid_decomp.h>
7+
#include <decomp_geometry/geometric_utils.h>
8+
9+
10+
#include <fstream>
11+
#include <boost/geometry.hpp>
12+
#include <boost/geometry/geometries/point_xy.hpp>
13+
#include <boost/geometry/geometries/polygon.hpp>
14+
15+
16+
#include <mpc_pkg/SFC_generate_cls.h>
17+
#include <nav_msgs/OccupancyGrid.h>
18+
#include <jsk_recognition_msgs/PolygonArray.h>
19+
#include <geometry_msgs/PolygonStamped.h>
20+
#include <geometry_msgs/Point32.h>
21+
#include <geometry_msgs/PoseStamped.h>
22+
#include <gazebo_msgs/ModelStates.h>
23+
#include <sensor_msgs/LaserScan.h>
24+
#include <sensor_msgs/PointCloud.h>
25+
#include <laser_geometry/laser_geometry.h>
26+
27+
#define PI 3.1415
28+
29+
SFCGenerateNode::SFCGenerateNode()
30+
:logger("SFC_generate", 20)
31+
{
32+
33+
scan_multi_sub = nh.subscribe("/abb/laser/scan_multi", 1, &SFCGenerateNode::scan_multiCB, this);
34+
point_cloud_pub = nh.advertise<sensor_msgs::PointCloud>("/SFC_generate_node/point_cloud", 10);
35+
polygon_pub = nh.advertise<jsk_recognition_msgs::PolygonArray>("/SFC_generate_node/free_polygon", 10);
36+
mpc_constraints_service = nh.advertiseService("/SFC_generate_node/get_constraints", &SFCGenerateNode::GetMpcConstraintsCB, this);
37+
38+
bool enable_logging;
39+
ros::param::get("/SFC_generate_enable_logging", enable_logging);
40+
logger.get_enable(enable_logging);
41+
}
42+
43+
44+
void SFCGenerateNode::scan_multiCB(const sensor_msgs::LaserScan::ConstPtr& data)
45+
{
46+
47+
// std::stringstream ss0;
48+
// ss0 << data->header.frame_id;
49+
// std::string str = ss0.str();
50+
51+
// auto output_msg = "laser scan frame id is " + str;
52+
// logger.log(output_msg, 1);
53+
54+
laser_geometry::LaserProjection projector_;
55+
sensor_msgs::PointCloud cloud;
56+
projector_.projectLaser(*data, cloud);
57+
58+
obs.clear();
59+
for (size_t i = 0; i < cloud.points.size(); i++)
60+
{
61+
Vecf<2> pt;
62+
63+
pt(0) = cloud.points[i].x;
64+
pt(1) = cloud.points[i].y;
65+
66+
Vecf<2> pt1, pt2, pt3, pt4;
67+
double radiu = 0.75;
68+
pt1(0) = pt(0) - radiu;
69+
pt1(1) = pt(1);
70+
71+
pt2(0) = pt(0) + radiu;
72+
pt2(1) = pt(1);
73+
74+
pt3(0) = pt(0);
75+
pt3(1) = pt(1) + radiu;
76+
77+
pt4(0) = pt(0);
78+
pt4(1) = pt(1) - radiu;
79+
80+
obs.push_back(pt);
81+
obs.push_back(pt1);
82+
obs.push_back(pt2);
83+
obs.push_back(pt3);
84+
obs.push_back(pt4);
85+
}
86+
87+
point_cloud_pub.publish(cloud);
88+
}
89+
90+
91+
bool SFCGenerateNode::GetMpcConstraintsCB(mpc_pkg::MpcConstraint::Request & req, mpc_pkg::MpcConstraint::Response & res)
92+
{
93+
logger.log("this is get mpc constraints CB");
94+
const Vec2f origin(- 4, - 4);
95+
const Vec2f range(8, 8);
96+
97+
Vec2f current_pose{req.current_state[0], req.current_state[1]};
98+
99+
path.clear();
100+
101+
path.push_back(Vec2f(0.0, 0.0));
102+
for (size_t i=0; i< req.point_list.size(); i++)
103+
{
104+
path.push_back(Vec2f( req.point_list[i].x, req.point_list[i].y));
105+
//std::cout<< "x=" << path[i+1](0) << "y="<<path[i+1](1) << std::endl;
106+
}
107+
108+
109+
// Initialize SeedDecomp2D
110+
EllipsoidDecomp2D decomp(origin, range);
111+
decomp.set_obs(obs);
112+
decomp.set_local_bbox(Vec2f(3, 3));
113+
decomp.dilate(path, 0);
114+
auto ellipsoids = decomp.get_ellipsoids();
115+
116+
jsk_recognition_msgs::PolygonArray polygon_list;
117+
polygon_list.header.frame_id = "base_link";
118+
119+
for(const auto& poly: decomp.get_polyhedrons()) {
120+
const auto vertices = cal_vertices(poly);
121+
122+
geometry_msgs::PolygonStamped polygon_stamped;
123+
polygon_stamped.header.frame_id = "base_link";
124+
for (size_t i = 0; i < vertices.size(); i++)
125+
{
126+
geometry_msgs::Point32 p;
127+
128+
p.x = vertices[i](0);
129+
p.y = vertices[i](1);
130+
p.z = 0;
131+
polygon_stamped.polygon.points.push_back(p);
132+
}
133+
134+
135+
polygon_list.polygons.push_back(polygon_stamped);
136+
}
137+
138+
139+
polygon_pub.publish(polygon_list);
140+
141+
res.polygon = polygon_list;
142+
return true;
143+
}

‎base_mpc/mpc_pkg/src/SFC_generate_node.cpp

+22
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,22 @@
1+
2+
3+
#include "mpc_pkg/SFC_generate_cls.h"
4+
#include <ros/ros.h>
5+
6+
int main(int argc, char **argv)
7+
{
8+
// ROS
9+
ros::init(argc, argv, "SFC_generate_node");
10+
ROS_INFO("\033[1;33m SFC generate node initialized! \033[0m");
11+
SFCGenerateNode sfc_generate_node;
12+
13+
ros::Rate rate(10);
14+
15+
// while(ros::ok())
16+
// {
17+
// rate.sleep();
18+
// }
19+
ros::spin();
20+
21+
return 0;
22+
}

‎base_mpc/mpc_pkg/src/goal_receiver_node.py

+85
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,85 @@
1+
#!/usr/bin/env python
2+
# -*- coding: utf-8 -*-
3+
4+
# standard module
5+
import rospy
6+
import tf2_ros
7+
from termcolor import colored
8+
9+
# custom module
10+
from ROS_visualization.ros_visuals import ROSMarkerPublisher
11+
from ROS_visualization.logger import Logger
12+
13+
# ros messages and services
14+
from geometry_msgs.msg import PoseStamped, Pose
15+
from std_srvs.srv import Empty, EmptyResponse, SetBool, SetBoolResponse
16+
from mpc_pkg.srv import GoalInfo, GoalInfoResponse
17+
18+
19+
class goal_receiver_node():
20+
def __init__(self):
21+
# parameters
22+
self.enable_logging = rospy.get_param("goal_receiver_enable_logging", False)
23+
self.radius = rospy.get_param("radius", 0.5)
24+
25+
# attribute
26+
self.goal_received = False
27+
self.goal_achieved = False
28+
self.goal_pose_in_map = PoseStamped()
29+
self.logger = Logger("goal_receiver", 1)
30+
31+
# define goal topic subscriber
32+
self.goal_subscriber = rospy.Subscriber("/move_base_simple/goal", PoseStamped, self.GoalCB)
33+
self.visuals = ROSMarkerPublisher('/goal_receiver/visuals', 100)
34+
self.set_goal_achieved_server = rospy.Service("/goal_receiver/set_goal_achieved", Empty, self.SetGoalAchievedCB)
35+
self.get_goal_info_server = rospy.Service("/goal_receiver/get_goal_info", GoalInfo, self.GetGoalInfoCB)
36+
37+
def visualize(self):
38+
if self.goal_received and not self.goal_achieved:
39+
circle_pose = Pose()
40+
circle_pose.position.x = self.goal_pose_in_map.pose.position.x
41+
circle_pose.position.y = self.goal_pose_in_map.pose.position.y
42+
43+
self.visual_goal = self.visuals.get_circle("map")
44+
self.visual_goal.set_scale(0.3 + self.radius, 0.3 + self.radius, 0.01)
45+
self.visual_goal.set_color(15, 0.5)
46+
47+
self.visual_goal.add_marker(circle_pose)
48+
49+
self.visuals.publish()
50+
51+
def GoalCB(self, msg):
52+
self.logger.log("started")
53+
54+
self.goal_pose_in_map = msg
55+
self.logger.log("goal pose in map:", type=1)
56+
self.logger.log(self.goal_pose_in_map, type=1)
57+
58+
self.goal_received = True
59+
self.goal_achieved = False
60+
self.logger.log("end")
61+
62+
def SetGoalAchievedCB(self, req):
63+
self.goal_achieved = True
64+
self.goal_received = False
65+
66+
res = EmptyResponse()
67+
return res
68+
69+
def GetGoalInfoCB(self, req):
70+
res = GoalInfoResponse()
71+
res.goal_received = self.goal_received
72+
res.goal_achieved = self.goal_achieved
73+
res.goal_pose_in_map = self.goal_pose_in_map
74+
return res
75+
76+
77+
if __name__ == "__main__":
78+
rospy.init_node("goal_receiver_node")
79+
print(colored("goal_receiver_node initialized", 'yellow'))
80+
goal_receiver = goal_receiver_node()
81+
rate = rospy.Rate(20)
82+
83+
while not rospy.is_shutdown():
84+
goal_receiver.visualize()
85+
rate.sleep()

‎base_mpc/mpc_pkg/src/logger.cpp

+71
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,71 @@
1+
2+
# include <iostream>
3+
# include <ros/ros.h>
4+
# include <string>
5+
# include "mpc_pkg/logger.h"
6+
7+
8+
Logger::Logger(std::string prefix, int log_frequence, bool enable_logging)
9+
:prefix(prefix), log_frequence(log_frequence), enable_logging(enable_logging)
10+
{
11+
std::string para_name;
12+
para_name = prefix + "_enable_logging";
13+
ros::param::get(para_name, enable_logging);
14+
}
15+
16+
17+
void Logger::log(const std::string& msg, int type)
18+
{
19+
// type
20+
// -1 -> error
21+
// 0 -> normal
22+
// 1 -> important
23+
if (enable_logging)
24+
{
25+
std::string prefix_temp = prefix;
26+
auto out_put_msg = prefix_temp + ": " + msg;
27+
if (type == 0 && log_counts % log_frequence == 0)
28+
{
29+
ROS_INFO(out_put_msg.c_str());
30+
}
31+
else if(type == 1)
32+
{
33+
ROS_BLUE_STREAM(out_put_msg.c_str());
34+
}
35+
36+
if(type == -1)
37+
{
38+
ROS_RED_STREAM(out_put_msg.c_str());
39+
}
40+
41+
std::string end_s = "end";
42+
if (!end_s.compare(msg))
43+
{
44+
log_counts += 1;
45+
}
46+
}
47+
48+
}
49+
50+
void Logger::get_enable(const bool& a)
51+
{
52+
enable_logging = a;
53+
}
54+
55+
std::ostream& pc::operator<<(std::ostream& os, PRINT_COLOR c)
56+
{
57+
switch(c)
58+
{
59+
case BLACK : os << "\033[1;30m"; break;
60+
case RED : os << "\033[1;31m"; break;
61+
case GREEN : os << "\033[1;32m"; break;
62+
case YELLOW : os << "\033[1;33m"; break;
63+
case BLUE : os << "\033[1;34m"; break;
64+
case MAGENTA : os << "\033[1;35m"; break;
65+
case CYAN : os << "\033[1;36m"; break;
66+
case WHITE : os << "\033[1;37m"; break;
67+
case ENDCOLOR : os << "\033[0m"; break;
68+
default : os << "\033[1;37m";
69+
}
70+
return os;
71+
}

‎base_mpc/mpc_pkg/src/mpc_node.py

+417
Large diffs are not rendered by default.

‎base_mpc/mpc_pkg/srv/GoalInfo.srv

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
---
2+
bool goal_received
3+
bool goal_achieved
4+
geometry_msgs/PoseStamped goal_pose_in_map

‎base_mpc/mpc_pkg/srv/MpcConstraint.srv

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
float32[] current_state
2+
geometry_msgs/Point[] point_list
3+
---
4+
jsk_recognition_msgs/PolygonArray polygon

‎base_mpc/mpc_pkg/srv/PoseInfo.srv

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
---
2+
bool pose_received
3+
geometry_msgs/PoseStamped pose_stamped_in_map

‎base_mpc/mpc_pkg/srv/TwistInfo.srv

+3
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,3 @@
1+
---
2+
bool twist_received
3+
geometry_msgs/TwistStamped twist_stamped_in_map

‎poetry.lock

+923-4
Some generated files are not rendered by default. Learn more about customizing how changed files appear on GitHub.

‎pyproject.toml

+6-1
Original file line numberDiff line numberDiff line change
@@ -7,9 +7,14 @@ readme = "README.md"
77
packages = [{include = "base_mpc"}]
88

99
[tool.poetry.dependencies]
10-
python = "^3.9"
10+
python = "^3.8"
1111
urdfenvs = "^0.8.15"
1212
robotmpcs = {git = "https://github.com/maxspahn/robot_mpcs.git", rev = "ft-updates"}
13+
rospkg = "^1.5.0"
14+
roslibpy = "^1.5.0"
15+
pypoman = "^1.0.0"
16+
termcolor = "^2.3.0"
17+
pyyaml = "^6.0.1"
1318

1419

1520
[build-system]

0 commit comments

Comments
 (0)
Please sign in to comment.